/** * Creates a new Jacobian for the kinematic starting from {@code ancestor} and ending at {@code descendant}. * * @param ancestor the predecessor of the first joint considered by this Jacobian. * @param descendant the successor of the last joint considered by this Jacobian. * @param jacobianFrame the frame in which the resulting twist of the end effector with respect to the base frame will be expressed. * @throws RuntimeException if the joint ordering is incorrect. */ public GeometricJacobian(RigidBody ancestor, RigidBody descendant, ReferenceFrame jacobianFrame) { this(ScrewTools.createJointPath(ancestor, descendant), jacobianFrame, true); }
public static InverseDynamicsJoint[] computeSupportJoints(RigidBody... bodies) { Set<InverseDynamicsJoint> supportSet = new LinkedHashSet<InverseDynamicsJoint>(); for (RigidBody rigidBody : bodies) { RigidBody rootBody = getRootBody(rigidBody); InverseDynamicsJoint[] jointPath = createJointPath(rootBody, rigidBody); supportSet.addAll(Arrays.asList(jointPath)); } return supportSet.toArray(new InverseDynamicsJoint[supportSet.size()]); }
/** * Find or create a Jacobian and register it in the MomentumBasedController. * It returns an jacobianId with which it is possible to find the Jacobian later with the method getJacobian(int jacobianId). * @param ancestor * @param descendant * @param jacobianFrame * @return */ public long getOrCreateGeometricJacobian(RigidBody ancestor, RigidBody descendant, ReferenceFrame jacobianFrame) { int numberOfJoints = ScrewTools.createJointPath(temporaryToStoreJointPath, ancestor, descendant); return getOrCreateGeometricJacobian(temporaryToStoreJointPath, numberOfJoints, jacobianFrame); }
public static OneDoFJoint[] createOneDoFJointPath(RigidBody start, RigidBody end) { return filterJoints(createJointPath(start, end), OneDoFJoint.class); }
@Override public void addConstraint(RigidBody body, DenseMatrix64F selectionMatrix) { constrainedBodiesAndSelectionMatrices.put(body, selectionMatrix); nConstraints += selectionMatrix.getNumRows(); InverseDynamicsJoint[] jointPath = ScrewTools.createJointPath(rootJoint.getPredecessor(), body); this.supportingBodyToJointPathMap.put(body, Arrays.asList(jointPath)); }
/** * Creates a new Jacobian for the open loop chain described by the ordered keys of {@code unitTwists}. * * @param unitTwists an ordered list of relative joint twists. * The order in which the twists are ordered will correspond to the order of the columns in the Jacobian * @param jacobianFrame the frame in which the resulting twist of the end effector with respect to the base frame will be expressed. * @param allowChangeFrame whether the feature {@link #changeFrame(ReferenceFrame)} will be available or not. * @throws RuntimeException if the joint ordering is incorrect. */ private GeometricJacobian(LinkedHashMap<InverseDynamicsJoint, List<Twist>> unitTwists, ReferenceFrame jacobianFrame, boolean allowChangeFrame) { this.joints = new InverseDynamicsJoint[unitTwists.size()]; unitTwists.keySet().toArray(joints); this.unitTwistMap = unitTwists; this.unitTwistList = extractUnitTwistList(unitTwists); this.jacobianFrame = jacobianFrame; this.jacobian = createJacobianMatrix(this.unitTwistMap); this.jointPathFromBaseToEndEffector = ScrewTools.createJointPath(getBase(), getEndEffector()); this.allowChangeFrame = allowChangeFrame; nameBasedHashCode = ScrewTools.computeGeometricJacobianNameBasedHashCode(joints, jacobianFrame, allowChangeFrame); }
if (hand != null) InverseDynamicsJoint[] armJoints = ScrewTools.createJointPath(controllerModel.getChest(), hand); OneDoFJoint[] filterArmJoints = ScrewTools.filterJoints(armJoints, OneDoFJoint.class); for (OneDoFJoint armJoint : filterArmJoints)
InverseDynamicsJoint[] jointPath = ScrewTools.createJointPath(fullRobotModel.getPelvis(), fullRobotModel.getFoot(robotSide)); actuatedJoints.addAll(Arrays.asList(jointPath));
OneDoFJoint[] armJoints = ScrewTools.filterJoints(ScrewTools.createJointPath(chest, hand), OneDoFJoint.class); OneDoFJoint elbowJoint = armJoints[3]; double jointSign = -Math.signum(elbowJoint.getJointLimitLower() + elbowJoint.getJointLimitUpper()); armZeroJointAngleConfigurationOffsets.put(robotSide, armZeroJointAngleConfigurationOffset); upperArmJoints.put(robotSide, ScrewTools.filterJoints(ScrewTools.createJointPath(chest, upperArmBody), OneDoFJoint.class)); upperArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(upperArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), inverseKinematicsForUpperArms.put(robotSide, inverseKinematicsForUpperArm); lowerArmJoints.put(robotSide, ScrewTools.filterJoints(ScrewTools.createJointPath(lowerArmBody, hand), OneDoFJoint.class)); lowerArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(lowerArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide),
base = robotModel.getPelvis(); RigidBody foot = robotModel.getFoot(robotSide); robotJoints = ScrewTools.filterJoints(ScrewTools.createJointPath(base, foot), OneDoFJoint.class); ikJoints = ScrewTools.filterJoints(ScrewTools.cloneJointPath(robotJoints), OneDoFJoint.class); jacobian = new GeometricJacobian(ikJoints, ikJoints[ikJoints.length - 1].getSuccessor().getBodyFixedFrame());