/** * 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(RigidBodyBasics ancestor, RigidBodyBasics descendant, ReferenceFrame jacobianFrame) { this(MultiBodySystemTools.createJointPath(ancestor, descendant), jacobianFrame, true); }
/** * Collects and returns all the joints located between the given {@code rigidBody} and the root * body. * <p> * WARNING: This method generates garbage. * </p> * * @param rigidBody the rigid-body to collect the support joints of. * @return the array containing the support joints of the given rigid-body. */ public static JointReadOnly[] collectSupportJoints(RigidBodyReadOnly rigidBody) { return createJointPath(getRootBody(rigidBody), rigidBody); }
/** * Collects and returns all the joints located between the given {@code rigidBody} and the root * body. * <p> * WARNING: This method generates garbage. * </p> * * @param rigidBody the rigid-body to collect the support joints of. * @return the array containing the support joints of the given rigid-body. */ public static JointBasics[] collectSupportJoints(RigidBodyBasics rigidBody) { return createJointPath(getRootBody(rigidBody), rigidBody); }
/** * Travels the multi-body system from {@code start} to {@code end} and stores in order the joints * that implement {@code OneDoFJointBasics} and that are in between and return them as an array. * <p> * WARNING: This method generates garbage. * </p> * * @param start the rigid-body from where to begin the collection of joints. * @param end the rigid-body where to stop the collection of joints. * @return the array of joints representing the path from {@code start} to {@code end}. */ public static OneDoFJointBasics[] createOneDoFJointPath(RigidBodyBasics start, RigidBodyBasics end) { return filterJoints(createJointPath(start, end), OneDoFJointBasics.class); }
/** * Travels the multi-body system from {@code start} to {@code end} and stores in order the joints * that are in between and return them as an array. * <p> * WARNING: This method generates garbage. * </p> * * @param start the rigid-body from where to begin the collection of joints. * @param end the rigid-body where to stop the collection of joints. * @return the array of joints representing the path from {@code start} to {@code end}, or * {@code null} if the given rigid-bodies are not part of the same multi-body system. */ public static JointBasics[] createJointPath(RigidBodyBasics start, RigidBodyBasics end) { return filterJoints(createJointPath((RigidBodyReadOnly) start, (RigidBodyReadOnly) end), JointBasics.class); }
@Override public void addConstraint(RigidBodyBasics body, DenseMatrix64F selectionMatrix) { constrainedBodiesAndSelectionMatrices.put(body, selectionMatrix); nConstraints += selectionMatrix.getNumRows(); JointBasics[] jointPath = MultiBodySystemTools.createJointPath(rootJoint.getPredecessor(), body); this.supportingBodyToJointPathMap.put(body, Arrays.asList(jointPath)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCreateJointPath() { int numberOfJoints = joints.size(), numberOfBodies = numberOfJoints + 1; RigidBodyBasics[] allBodies = new RigidBodyBasics[numberOfBodies]; allBodies[0] = elevator; for(int i = 0; i < numberOfJoints; i++) { allBodies[i+1] = joints.get(i).getSuccessor(); } RigidBodyBasics start = allBodies[0] , end = allBodies[allBodies.length - 1]; JointBasics[] jointPath = MultiBodySystemTools.createJointPath(start, end); for(int i = 0; i < jointPath.length; i++) { assertTrue(jointPath[i].getName().equalsIgnoreCase("chainCjoint" + i)); } }
/** * Creates the Jacobian for the kinematic chain described by the given joints. These joints have * to be ordered and going downstream (the first joint has to be the closest joint to the root of * the system and the last the farthest). * * @param joints the joints that this Jacobian considers. * @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. */ public GeometricJacobian(JointBasics[] joints, ReferenceFrame jacobianFrame, boolean allowChangeFrame) { checkJointOrder(joints); this.joints = new JointBasics[joints.length]; System.arraycopy(joints, 0, this.joints, 0, joints.length); this.jacobianFrame = jacobianFrame; this.jacobian = new DenseMatrix64F(SpatialVector.SIZE, MultiBodySystemTools.computeDegreesOfFreedom(joints)); this.jointPathFromBaseToEndEffector = MultiBodySystemTools.createJointPath(getBase(), getEndEffector()); this.allowChangeFrame = allowChangeFrame; hashCode = ScrewTools.computeGeometricJacobianHashCode(joints, jacobianFrame, allowChangeFrame); }
if (hand != null) JointBasics[] armJoints = MultiBodySystemTools.createJointPath(controllerRobotModel.getChest(), hand); OneDoFJointBasics[] filterArmJoints = MultiBodySystemTools.filterJoints(armJoints, OneDoFJointBasics.class); for (OneDoFJointBasics armJoint : filterArmJoints)
JointBasics[] controlledJoints = MultiBodySystemTools.createJointPath(pelvis, endEffector); GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, pelvis.getBodyFixedFrame()); jacobian.compute();
Wrench wrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); JointBasics[] controlledJoints = MultiBodySystemTools.createJointPath(pelvis, foot); GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, pelvis.getBodyFixedFrame()); jacobian.compute();
JointBasics[] jointPath = MultiBodySystemTools.createJointPath(fullRobotModel.getPelvis(), fullRobotModel.getFoot(robotSide)); actuatedJoints.addAll(Arrays.asList(jointPath));
OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.createJointPath(chest, hand), OneDoFJointBasics.class); OneDoFJointBasics elbowJoint = armJoints[3]; double jointSign = -Math.signum(elbowJoint.getJointLimitLower() + elbowJoint.getJointLimitUpper()); armZeroJointAngleConfigurationOffsets.put(robotSide, armZeroJointAngleConfigurationOffset); upperArmJoints.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemTools.createJointPath(chest, upperArmBody), OneDoFJointBasics.class)); upperArmJointsClone.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemFactories.cloneKinematicChain(upperArmJoints.get(robotSide)), OneDoFJointBasics.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), inverseKinematicsForUpperArms.put(robotSide, inverseKinematicsForUpperArm); lowerArmJoints.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemTools.createJointPath(lowerArmBody, hand), OneDoFJointBasics.class)); lowerArmJointsClone.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemFactories.cloneKinematicChain(lowerArmJoints.get(robotSide)), OneDoFJointBasics.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide),
Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); JointBasics[] controlledJoints = MultiBodySystemTools.createJointPath(pelvis, endEffector); GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, pelvis.getBodyFixedFrame()); jacobian.compute();