public ReferenceFrame getFrameAfterParentJoint() { return ankle.getFrameAfterJoint(); }
private void addJoint(CollisionBoxProvider collissionBoxProvider, InverseDynamicsJoint joint) { List<CollisionShape> collisionMesh = collissionBoxProvider.getCollisionMesh(joint.getName()); if (collisionMesh != null) { trackingCollisionShapes.add(new TrackingCollisionShape(joint.getFrameAfterJoint(), collisionMesh)); } else { System.err.println(joint + " does not have a collission mesh"); } }
/** * Returns the end-effector {@code RigidBody} of this Jacobian. * The end-effector is the successor of the last joint this Jacobian considers. * * @return the end-effector of this jacobian. */ public RigidBody getEndEffector() { return joints[joints.length - 1].getSuccessor(); }
public static void getJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositionsToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { joint.getConfigurationMatrix(jointPositionsToPack, rowStart); rowStart += joint.getDegreesOfFreedom(); if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint) rowStart++; // Because of stupid quaternions } }
public static void getJointVelocitiesMatrix(Iterable<? extends InverseDynamicsJoint> joints, DenseMatrix64F jointVelocitiesMatrixToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { int dof = joint.getDegreesOfFreedom(); joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart); rowStart += dof; } }
public static void setJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositions) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { joint.setConfiguration(jointPositions, rowStart); rowStart += joint.getDegreesOfFreedom(); if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint) rowStart++; // Because of stupid quaternions } }
private void printJointInformation(InverseDynamicsJoint joint, StringBuffer buffer) String jointName = joint.getName(); InverseDynamicsJoint parentJoint = joint.getPredecessor().getParentJoint(); if (parentJoint == null) ReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint(); ReferenceFrame frameAfterParentJoint = parentJoint.getFrameAfterJoint(); RigidBodyTransform comOffsetTransform = frameBeforeJoint.getTransformToDesiredFrame(frameAfterParentJoint); comOffsetTransform.getTranslation(jointOffset); RigidBody linkRigidBody = joint.getSuccessor(); printLinkInformation(linkRigidBody, buffer);
private static final void areJointsTheSame(InverseDynamicsJoint originalJoint, InverseDynamicsJoint targetJoint) { if(!(originalJoint.getClass().equals(targetJoint.getClass()) && originalJoint.getName().equals(targetJoint.getName()))) { throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint); } } }
private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (InverseDynamicsJoint joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.set(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } }
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
if (currentBody == joint.getPredecessor()) joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor(); else if (currentBody == joint.getSuccessor()) joint.getSuccessorTwist(jointTwist); currentBody = joint.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); }
/** * 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(); }
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 computeDegreesOfFreedom(InverseDynamicsJoint[] jointList) { int ret = 0; for (InverseDynamicsJoint joint : jointList) { ret += joint.getDegreesOfFreedom(); } return ret; }
public void compute() { int column = 0; for (int i = 0; i < rigidBodyList.size(); i++) { comScaledByMassMapIsUpdated.get(rigidBodyList.get(i)).setValue(false); } for (InverseDynamicsJoint joint : joints) { RigidBody childBody = joint.getSuccessor(); FramePoint comPositionScaledByMass = getCoMScaledByMass(childBody); double subTreeMass = getSubTreeMass(childBody); GeometricJacobian motionSubspace = joint.getMotionSubspace(); final List<Twist> allTwists = motionSubspace.getAllUnitTwists(); for(int i = 0; i < allTwists.size(); i++) { tempUnitTwist.set(allTwists.get(i)); tempUnitTwist.changeFrame(rootFrame); setColumn(tempUnitTwist, comPositionScaledByMass, subTreeMass, column); column++; } } CommonOps.scale(inverseTotalMass, jacobianMatrix); }
private void computeJointWrenchesAndTorques() { for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) { InverseDynamicsJoint joint = allJoints.get(jointIndex); RigidBody successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); jointWrench.set(netWrenches.get(successor)); Wrench externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); List<InverseDynamicsJoint> childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { InverseDynamicsJoint child = childrenJoints.get(childIndex); if (!jointsToIgnore.contains(child)) { Wrench wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); wrenchExertedByChild.set(wrenchExertedOnChild); wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); wrenchExertedByChild.scale(-1.0); // Action = -reaction wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); jointWrench.sub(wrenchExertedByChild); } } joint.setTorqueFromWrench(jointWrench); } }
private void setDesiredAccelerationsToZero() { for (InverseDynamicsJoint joint : jointsInOrder) { joint.setDesiredAccelerationToZero(); joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0); } }
public static void setVelocities(InverseDynamicsJoint[] jointList, DenseMatrix64F jointVelocities) { int rowStart = 0; for (InverseDynamicsJoint joint : jointList) { joint.setVelocity(jointVelocities, rowStart); rowStart += joint.getDegreesOfFreedom(); } }
public static void getDesiredJointAccelerationsMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F desiredJointAccelerationsMatrixToPack) { int rowStart = 0; for (InverseDynamicsJoint joint : joints) { int dof = joint.getDegreesOfFreedom(); joint.getDesiredAccelerationMatrix(desiredJointAccelerationsMatrixToPack, rowStart); rowStart += dof; } }