/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
{
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
scsJoint.setTau(idJoint.getTau());
}
}
/** SCS Robot --> ID Robot Send positions and velocities. */
public void updatePositionsIDrobot() {
// Body info
bodyJointID.setPosition(
bodyJointSCS.getQ_t1().getDoubleValue(), 0.0, bodyJointSCS.getQ_t2().getDoubleValue());
bodyJointID.setRotation(0.0, bodyJointSCS.getQ_rot().getDoubleValue(), 0.0);
double[] velocityArray = new double[6];
velocityArray[0] = 0.0; // yaw
velocityArray[1] = bodyJointSCS.getQd_rot().getDoubleValue(); // pitch
velocityArray[2] = 0.0; // roll
velocityArray[3] = bodyJointSCS.getQd_t1().getDoubleValue(); // x
velocityArray[4] = 0.0; // y
velocityArray[5] = bodyJointSCS.getQd_t2().getDoubleValue(); // z
DenseMatrix64F velocities = new DenseMatrix64F(6, 1, true, velocityArray);
bodyJointID.setVelocity(velocities, 0);
// Leg and foot info (hip, knee and ankle)
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue());
idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
}
elevator.updateFramesRecursively();
// Get the body coordinates
bodyPosition = new FramePoint();
bodyPosition.setToZero(bodyJointID.getFrameAfterJoint());
bodyPosition.changeFrame(worldFrame);
}
public void startComputation() {
for (OneDoFJoint joint : oneDoFJoints) {
if (joint == null) throw new RuntimeException();
ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint);
ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint);
double positionSensorData = positionSensorPort.getData()[0];
double velocitySensorData = velocitySensorPort.getData()[0];
joint.setQ(positionSensorData);
joint.setQd(velocitySensorData);
joint.setQdd(joint.getQddDesired());
}
// TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet?
FullInverseDynamicsStructure inverseDynamicsStructure =
inverseDynamicsStructureOutputPort.getData();
TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator();
SpatialAccelerationCalculator spatialAccelerationCalculator =
inverseDynamicsStructure.getSpatialAccelerationCalculator();
twistCalculator.getRootBody().updateFramesRecursively();
twistCalculator.compute();
spatialAccelerationCalculator.compute();
inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure);
}
@Override
public void write() {
for (int i = 0; i < revoluteJoints.size(); i++) {
ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
OneDoFJoint revoluteJoint = jointPair.getRight();
pinJoint.setTau(revoluteJoint.getTau());
}
}
public void updateRobotConfigurationBasedOnFullRobotModel() {
for (int i = 0; i < revoluteJoints.size(); i++) {
ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
OneDoFJoint revoluteJoint = jointPair.getRight();
pinJoint.setQ(revoluteJoint.getQ());
}
FloatingJoint floatingJoint = rootJointPair.getLeft();
FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight();
RigidBodyTransform transform = sixDoFJoint.getJointTransform3D();
floatingJoint.setRotationAndTranslation(transform);
}
@Override
public void setFullRobotModel(FullRobotModel fullRobotModel) {
revoluteJoints.clear();
OneDoFJoint[] revoluteJointsArray = fullRobotModel.getOneDoFJoints();
for (OneDoFJoint revoluteJoint : revoluteJointsArray) {
String name = revoluteJoint.getName();
OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name);
ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair =
new ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint>(oneDoFJoint, revoluteJoint);
this.revoluteJoints.add(jointPair);
}
rootJointPair =
new ImmutablePair<FloatingJoint, FloatingInverseDynamicsJoint>(
robot.getRootJoint(), fullRobotModel.getRootJoint());
}
@Override
public void writeAfterController(long timestamp) {
jointCommand.clear();
jointCommand.putLong(estimatorTicksPerControlTick);
jointCommand.putLong(timestamp);
jointCommand.putLong(estimatorFrequencyInHz);
for (int i = 0; i < joints.size(); i++) {
OneDoFJoint joint = joints.get(i);
if (fingerJointMap == null || !fingerJointMap.containsKey(joint.getName())) {
if (joint.isUnderPositionControl()) jointCommand.putDouble(joint.getqDesired());
else jointCommand.putDouble(joint.getTau());
} else
jointCommand.putDouble(
fingerJointMap
.get(joint.getName())
.getqDesired()); // fingers are always position controlled
}
jointCommand.flip();
try {
while (jointCommand.hasRemaining()) {
channel.write(jointCommand);
}
} catch (IOException e) {
e.printStackTrace();
}
}
public void process(long timestamp) {
for (WandererJoint joint : WandererJoint.values) {
AcsellJointState jointState = state.getJointState(joint);
OneDoFJoint oneDoFJoint = jointMap.get(joint);
RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);
oneDoFJoint.setQ(jointState.getQ());
oneDoFJoint.setQd(jointState.getQd());
state.updateRawSensorData(joint, rawSensor);
}
AcsellXSensState xSensState = state.getXSensState();
xSensState.getQuaternion(rotation);
// rootJoint.setRotation(rotation);
rootJoint.updateFramesRecursively();
if (!initialized) {
controller.initialize(timestamp);
initialized = true;
}
controller.doControl(timestamp);
if (controller.turnOutputOn()) {
command.enableActuators();
}
for (WandererJoint joint : WandererJoint.values) {
OneDoFJoint oneDoFJoint = jointMap.get(joint);
RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);
AcsellJointCommand jointCommand = command.getAcsellJointCommand(joint);
jointCommand.setTauDesired(oneDoFJoint.getTau(), 0.0, rawSensor);
jointCommand.setDamping(oneDoFJoint.getKd());
}
outputWriter.write();
if (visualizer != null) {
visualizer.update(timestamp);
}
}
public double getValue() {
return joint.getQ();
}
