@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } }
@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.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } } }
@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.setQ(revoluteJoint.getqDesired()); pinJoint.setQd(revoluteJoint.getQdDesired()); pinJoint.setQdd(revoluteJoint.getQddDesired()); } } }
public void updateRobotConfigurationBasedOnJointDesiredOutputPositions() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); double q = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredPosition(); pinJoint.setQ(q); double qd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredVelocity(); pinJoint.setQd(qd); double qdd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredAcceleration(); pinJoint.setQdd(qdd); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : oneDoFJointPairList) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } if (rootJointPair != null) { FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingJointBasics sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); } } }
pinJoint.setQdd(jointAcceleration);
oneDoFJointsB[jointIndex].setQdd(oneDoFJointsA[jointIndex].getQDD());