/** * 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(); }
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); }
public static RigidBody[] computeSuccessors(InverseDynamicsJoint... joints) { RigidBody[] ret = new RigidBody[joints.length]; for (int i = 0; i < joints.length; i++) { InverseDynamicsJoint joint = joints[i]; ret[i] = joint.getSuccessor(); } return ret; }
private void populateMapsAndLists() { accelerations.put(rootBody, new SpatialAccelerationVector()); for (InverseDynamicsJoint joint : allJoints) { if (joint.getSuccessor() != null) { accelerations.put(joint.getSuccessor(), new SpatialAccelerationVector()); } } } }
private void addAllSuccedingJoints(RigidBody body) { for (InverseDynamicsJoint inverseDynamicsJoint : body.getChildrenJoints()) { if (inverseDynamicsJoint.getSuccessor() != null) { allJoints.add(inverseDynamicsJoint); addAllSuccedingJoints(inverseDynamicsJoint.getSuccessor()); } } }
public static void computeRigidBodiesAfterThisJoint(ArrayList<RigidBody> rigidBodySuccessorsToPack, InverseDynamicsJoint... joints) { ArrayList<InverseDynamicsJoint> jointStack = new ArrayList<InverseDynamicsJoint>(); for (InverseDynamicsJoint joint : joints) { jointStack.add(joint); } while (!jointStack.isEmpty()) { InverseDynamicsJoint currentJoint = jointStack.remove(0); rigidBodySuccessorsToPack.add(currentJoint.getSuccessor()); RigidBody currentBody = currentJoint.getSuccessor(); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { jointStack.add(joint); } } }
public static InverseDynamicsJoint[] computeSubtreeJoints(List<RigidBody> rootBodies) { ArrayList<InverseDynamicsJoint> subtree = new ArrayList<InverseDynamicsJoint>(); ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>(); rigidBodyStack.addAll(rootBodies); while (!rigidBodyStack.isEmpty()) { RigidBody currentBody = rigidBodyStack.remove(0); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { RigidBody successor = joint.getSuccessor(); rigidBodyStack.add(successor); subtree.add(joint); } } InverseDynamicsJoint[] ret = new InverseDynamicsJoint[subtree.size()]; return subtree.toArray(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 CentroidalMomentumMatrix(RigidBody rootBody, ReferenceFrame centerOfMassFrame) { this.jointList = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.centerOfMassFrame = centerOfMassFrame; int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointList); this.centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); this.unitMomenta = new Momentum[nDegreesOfFreedom]; for (int i = 0; i < nDegreesOfFreedom; i++) unitMomenta[i] = new Momentum(centerOfMassFrame); isAncestorMapping = new boolean[jointList.length][jointList.length]; for (int j = 0; j < jointList.length; j++) { RigidBody columnRigidBody = jointList[j].getSuccessor(); for (int i = 0; i < jointList.length; i++) { RigidBody rowRigidBody = jointList[i].getSuccessor(); isAncestorMapping[i][j] = ScrewTools.isAncestor(rowRigidBody, columnRigidBody); } } }
private double getSubTreeMass(RigidBody rigidBody) { if (!subTreeMassMap.containsKey(rigidBody)) subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); MutableDouble subTreeMass = subTreeMassMap.get(rigidBody); if (subTreeMass.doubleValue() > 0.0) { return subTreeMass.doubleValue(); } else { double curSubTreeMass = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { double childSubTreeMass = getSubTreeMass(childrenJoints.get(i).getSuccessor()); curSubTreeMass = curSubTreeMass + childSubTreeMass; } subTreeMass.setValue(curSubTreeMass); return curSubTreeMass; } }
public static double computeSubTreeMass(RigidBody rootBody) { RigidBodyInertia inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (InverseDynamicsJoint childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
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); } }
public ExternalWrenchHandler(double gravityZ, ReferenceFrame centerOfMassFrame, InverseDynamicsJoint rootJoint, List<? extends ContactablePlaneBody> contactablePlaneBodies) { this.centerOfMassFrame = centerOfMassFrame; MathTools.checkIfInRange(gravityZ, 0.0, Double.POSITIVE_INFINITY); this.contactablePlaneBodies = new ArrayList<>(contactablePlaneBodies); gravitationalWrench = new SpatialForceVector(centerOfMassFrame); double totalMass = TotalMassCalculator.computeMass(ScrewTools.computeSupportAndSubtreeSuccessors(rootJoint.getSuccessor())); gravitationalWrench.setLinearPartZ(-gravityZ * totalMass); totalWrenchAlreadyApplied = new SpatialForceVector(centerOfMassFrame); for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = this.contactablePlaneBodies.get(i).getRigidBody(); externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); } }
public static NumericalInverseKinematicsCalculator createIKCalculator(InverseDynamicsJoint[] jointsToControl, int maxIterations) { InverseDynamicsJoint[] cloneOfControlledJoints = ScrewTools.cloneJointPath(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBody cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
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); }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
public SideDependentList<ContactablePlaneBody> createHandContactableBodies(RigidBody rootBody) { if (namesOfJointsBeforeHands == null) return null; InverseDynamicsJoint[] allJoints = ScrewTools.computeSupportAndSubtreeJoints(rootBody); SideDependentList<ContactablePlaneBody> handContactableBodies = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { InverseDynamicsJoint[] jointBeforeHandArray = ScrewTools.findJointsWithNames(allJoints, namesOfJointsBeforeHands.get(robotSide)); if (jointBeforeHandArray.length != 1) throw new RuntimeException("Incorrect number of joints before hand found: " + jointBeforeHandArray.length); RigidBody hand = jointBeforeHandArray[0].getSuccessor(); String name = robotSide.getCamelCaseNameForStartOfExpression() + "HandContact"; ListOfPointsContactablePlaneBody handContactableBody = createListOfPointsContactablePlaneBody(name, hand, handContactPointTransforms.get(robotSide), handContactPoints.get(robotSide)); handContactableBodies.put(robotSide, handContactableBody); } return handContactableBodies; }
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()); } }
private StateEstimatorSensorDefinitions buildStateEstimatorSensorDefinitions(FullRobotModel fullRobotModel) { InverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); ForceSensorDefinition[] forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); StateEstimatorSensorDefinitions stateEstimatorSensorDefinitions = new StateEstimatorSensorDefinitions(); for (InverseDynamicsJoint joint : ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor())) { if (joint instanceof OneDoFJoint) { OneDoFJoint oneDoFJoint = (OneDoFJoint) joint; stateEstimatorSensorDefinitions.addJointSensorDefinition(oneDoFJoint); } } for (IMUDefinition imuDefinition : imuDefinitions) { stateEstimatorSensorDefinitions.addIMUSensorDefinition(imuDefinition); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { stateEstimatorSensorDefinitions.addForceSensorDefinition(forceSensorDefinition); } return stateEstimatorSensorDefinitions; }
private void findMinimumAndMaximumMassOfRigidBodies(RigidBody body) { RigidBodyInertia inertia = body.getInertia(); if (inertia != null) { double mass = body.getInertia().getMass(); if (mass < minimumMassOfRigidBodies.getDoubleValue() && mass > 1e-3) minimumMassOfRigidBodies.set(mass); if (mass > maximumMassOfRigidBodies.getDoubleValue()) maximumMassOfRigidBodies.set(mass); } if (body.hasChildrenJoints()) { List<InverseDynamicsJoint> childJoints = body.getChildrenJoints(); for (InverseDynamicsJoint joint : childJoints) { RigidBody nextBody = joint.getSuccessor(); if (nextBody != null) findMinimumAndMaximumMassOfRigidBodies(nextBody); } } }