private void updateFrames() { localJoints[0].getPredecessor().updateFramesRecursively(); }
private void updateRobotFrames() { robotArmJoints[0].getPredecessor().updateFramesRecursively(); }
public void addJointsForControl(RigidBody controlledBody, OneDoFJoint[] jointsToUse) { // check joint order int length = jointsToUse.length; if (length > 0) { boolean rightOrder = true; if (length > 1) ScrewTools.isAncestor(jointsToUse[1].getPredecessor(), jointsToUse[0].getPredecessor()); OneDoFJoint[] orderedJointsToUse; if (rightOrder) orderedJointsToUse = jointsToUse; else { orderedJointsToUse = new OneDoFJoint[length]; for (int i = 0; i < length; i++) orderedJointsToUse[i] = jointsToUse[length - 1 - i]; } jointChainsForControl.get(controlledBody).add(orderedJointsToUse); for (int i = 0; i < length; i++) if (!controlledJoints.contains(orderedJointsToUse[i])) controlledJoints.add(orderedJointsToUse[i]); numberOfControlledJoints = controlledJoints.size(); } }
public void registerControlledBody(RigidBody controlledBody, RigidBody baseOfControl) { OneDoFJoint[] joints = ScrewTools.createOneDoFJointPath(baseOfControl, controlledBody); if (joints.length > 1) { if (ScrewTools.isAncestor(joints[1].getPredecessor(), joints[0].getPredecessor())) registerControlledBody(controlledBody, joints); else // need to reorder them { int size = joints.length; OneDoFJoint[] newJoints = new OneDoFJoint[size]; for (int i = 0; i < size; i++) { newJoints[i] = joints[size - i - 1]; } registerControlledBody(controlledBody, newJoints); } } else { registerControlledBody(controlledBody, joints); } }
RigidBody predecessorOriginal = jointOriginal.getPredecessor(); RigidBody predecessorCopy = originalToClonedRigidBodies.get(predecessorOriginal);
final RigidBody thigh = fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH).getPredecessor(); createMassAndCoMOffsetCorruptors(namePrefix, thighName, thigh);
elbowJointSign.put(robotSide, jointSign); RigidBody upperArmBody = elbowJoint.getPredecessor(); RigidBody lowerArmBody = elbowJoint.getSuccessor();