/** 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 setFingerJointsProvider(
SideDependentList<List<OneDegreeOfFreedomJoint>> allFingerJoints) {
fingerJointMap = new HashMap<String, OneDegreeOfFreedomJoint>();
for (RobotSide robotSide : RobotSide.values) {
for (OneDegreeOfFreedomJoint joint : allFingerJoints.get(robotSide)) {
fingerJointMap.put(joint.getName(), joint);
}
}
}
@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);
}