/** * Returns the base {@code RigidBody} of this Jacobian. * The base is the predecessor of the first joint that this Jacobian considers. * * @return the base of this Jacobian. */ public RigidBody getBase() { return joints[0].getPredecessor(); }
private void computeSuccessorAcceleration(InverseDynamicsJoint joint) { RigidBody predecessor = joint.getPredecessor(); RigidBody successor = joint.getSuccessor(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); joint.getPredecessorTwist(tempJointTwist); if (!doVelocityTerms) tempJointTwist.setToZero(); if (useDesireds) joint.getDesiredSuccessorAcceleration(tempJointAcceleration); else joint.getSuccessorAcceleration(tempJointAcceleration); if (!doAccelerationTerms) tempJointAcceleration.setToZero(); twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor); if (!doVelocityTerms) tempTwistFromWorld.setToZero(); SpatialAccelerationVector successorAcceleration = accelerations.get(successor); successorAcceleration.set(accelerations.get(predecessor)); successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld); successorAcceleration.add(tempJointAcceleration); }
private static void checkJointOrder(InverseDynamicsJoint[] joints) { for (int i = 1; i < joints.length; i++) { InverseDynamicsJoint joint = joints[i]; InverseDynamicsJoint previousJoint = joints[i - 1]; if (ScrewTools.isAncestor(previousJoint.getPredecessor(), joint.getPredecessor())) throw new RuntimeException("joints must be in order from ancestor to descendant"); } }
public void computeJacobianDerivativeTerm(SpatialAccelerationVector accelerationToPack) { ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame(); twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); for (int i = jacobian.getJointsInOrder().length - 1; i >= 0; i--) { twistOfCurrentWithRespectToBase.changeFrame(currentBody.getBodyFixedFrame()); InverseDynamicsJoint joint = jacobian.getJointsInOrder()[i]; if (currentBody == joint.getPredecessor()) { joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor(); } else { joint.getSuccessorTwist(jointTwist); currentBody = joint.getPredecessor(); } zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack); accelerationToPack.set(zeroAcceleration); jointTwist.invert(); twistOfCurrentWithRespectToBase.add(jointTwist); } accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame); }
private RigidBody addAllPrecedingJoints(RigidBody body) { ArrayList<InverseDynamicsJoint> jointsTillRootBody = new ArrayList<InverseDynamicsJoint>(); RigidBody currentBody = body; while (currentBody.getParentJoint() != null) { jointsTillRootBody.add(currentBody.getParentJoint()); currentBody = currentBody.getParentJoint().getPredecessor(); } for (int i = jointsTillRootBody.size() - 1; i >= 0; i--) { allJoints.add(jointsTillRootBody.get(i)); } return currentBody; }
if (currentBody == joint.getPredecessor()) currentBody = joint.getPredecessor();
public static boolean isAncestor(RigidBody candidateDescendant, RigidBody ancestor) { RigidBody currentBody = candidateDescendant; while (!currentBody.isRootBody()) { if (currentBody == ancestor) { return true; } currentBody = currentBody.getParentJoint().getPredecessor(); } return currentBody == ancestor; }
public static RigidBody getRootBody(RigidBody body) { RigidBody ret = body; while (ret.getParentJoint() != null) { ret = ret.getParentJoint().getPredecessor(); } return ret; }
public static int computeDistanceToAncestor(RigidBody descendant, RigidBody ancestor) { int ret = 0; RigidBody currentBody = descendant; while (!currentBody.isRootBody() && (currentBody != ancestor)) { ret++; currentBody = currentBody.getParentJoint().getPredecessor(); } if (currentBody != ancestor) ret = -1; return ret; }
public static RigidBody[] computeSubtreeSuccessors(InverseDynamicsJoint... joints) { ArrayList<RigidBody> rigidBodySuccessors = new ArrayList<RigidBody>(); ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>(); for (InverseDynamicsJoint joint : joints) { rigidBodyStack.add(joint.getPredecessor()); } while (!rigidBodyStack.isEmpty()) { RigidBody currentBody = rigidBodyStack.remove(0); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { rigidBodyStack.add(joint.getSuccessor()); rigidBodySuccessors.add(joint.getSuccessor()); } } RigidBody[] ret = new RigidBody[rigidBodySuccessors.size()]; return rigidBodySuccessors.toArray(ret); }
public static int[] createParentMap(RigidBody[] allRigidBodiesInOrder) { int[] parentMap = new int[allRigidBodiesInOrder.length]; List<RigidBody> rigidBodiesInOrderList = Arrays.asList(allRigidBodiesInOrder); // this doesn't have to be fast for (int i = 0; i < allRigidBodiesInOrder.length; i++) { RigidBody currentBody = allRigidBodiesInOrder[i]; if (currentBody.isRootBody()) { parentMap[i] = -1; } else { RigidBody parentBody = currentBody.getParentJoint().getPredecessor(); parentMap[i] = rigidBodiesInOrderList.indexOf(parentBody); } } return parentMap; }
public static void computeRigidBodiesFromRootToThisJoint(ArrayList<RigidBody> rigidBodySuccessorsToPack, InverseDynamicsJoint joint) { RigidBody predecessorBody = joint.getPredecessor(); if (predecessorBody == null) return; if (predecessorBody.isRootBody()) return; rigidBodySuccessorsToPack.add(predecessorBody); InverseDynamicsJoint parentJoint = predecessorBody.getParentJoint(); if (parentJoint == null) return; computeRigidBodiesFromRootToThisJoint(rigidBodySuccessorsToPack, parentJoint); }
public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
public static InverseDynamicsJoint[] createJointPath(RigidBody start, RigidBody end) { boolean flip = false; RigidBody descendant = start; RigidBody ancestor = end; int pathLength = computeDistanceToAncestor(descendant, ancestor); if (pathLength < 0) { flip = true; descendant = end; ancestor = start; pathLength = computeDistanceToAncestor(end, start); } InverseDynamicsJoint[] ret = new InverseDynamicsJoint[pathLength]; RigidBody currentBody = descendant; int i = 0; while (currentBody != ancestor) { int j = flip ? pathLength - 1 - i : i; InverseDynamicsJoint parentJoint = currentBody.getParentJoint(); ret[j] = parentJoint; currentBody = parentJoint.getPredecessor(); i++; } return ret; }
InverseDynamicsJoint parentJoint = currentBody.getParentJoint(); jointPathToPack[j] = parentJoint; currentBody = parentJoint.getPredecessor(); i++;
InverseDynamicsJoint parentJoint = joint.getPredecessor().getParentJoint(); if (parentJoint == null)
RigidBody predecessor = parentJoint.getPredecessor(); Twist twistOfPredecessor = computeOrGetTwistOfBody(predecessor); twist = assignAndGetEmptyTwist(rigidBody);