/** * Returns the body fixed frame of the end-effector {@code RigidBody} of this Jacobian. The * end-effector is the successor of the last joint this Jacobian considers. * * @return the body fixed frame of the end-effector. */ public ReferenceFrame getEndEffectorFrame() { return getEndEffector().getBodyFixedFrame(); }
twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); InverseDynamicsJoint[] jointPathFromBaseToEndEffector = jacobian.getJointPathFromBaseToEndEffector(); for (int i = jointPathFromBaseToEndEffector.length - 1; i >= 0; i--)
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); }
/** * Returns the body fixed frame of the end-effector {@code RigidBody} of this Jacobian. * The end-effector is the successor of the last joint this Jacobian considers. * * @return the body fixed frame of the end-effector. */ public ReferenceFrame getEndEffectorFrame() { return getEndEffector().getBodyFixedFrame(); }
public String getShortInfo() { return "Jacobian, end effector = " + getEndEffector() + ", base = " + getBase() + ", expressed in " + getJacobianFrame(); }
public String getShortInfo() { return "Jacobian, end effector = " + getEndEffector() + ", base = " + getBase() + ", expressed in " + getJacobianFrame(); }
/** * 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); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.invert(transformShoulderToEndEffector); transformEndEffectorToDesired.multiply(transformEndEffectorToShoulder, transformShoulderToDesired); }
public DesiredJointAccelerationCalculatorOld(GeometricJacobian jacobian, JacobianSolver jacobianSolver) { this.jointAccelerations = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jacobian.getJointsInOrder()), 1); this.sign = ScrewTools.isAncestor(jacobian.getEndEffector(), jacobian.getBase()) ? 1 : -1; // because the sign of a joint acceleration changes when you switch a joint's 'direction' in the chain this.jacobian = jacobian; this.jacobianSolver = jacobianSolver; }
/** * 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); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
public void compute(PointJacobian pointJacobian, FrameVector pPointVelocity) { GeometricJacobian jacobian = pointJacobian.getGeometricJacobian(); bodyFixedPoint.setIncludingFrame(pointJacobian.getPoint()); bodyFixedPoint.changeFrame(jacobian.getBaseFrame()); twistCalculator.getRelativeTwist(twist, jacobian.getBase(), jacobian.getEndEffector()); convectiveTermCalculator.computeJacobianDerivativeTerm(jacobian, convectiveTerm); convectiveTerm.changeFrame(jacobian.getBaseFrame(), twist, twist); twist.changeFrame(jacobian.getBaseFrame()); convectiveTerm.getAccelerationOfPointFixedInBodyFrame(twist, bodyFixedPoint, pPointVelocity); // bodyFixedPointVelocity.setToZero(jacobian.getBaseFrame()); // twist.packVelocityOfPointFixedInBodyFrame(bodyFixedPointVelocity, bodyFixedPoint); // // twist.packAngularPart(tempVector); // tempVector.cross(tempVector, bodyFixedPointVelocity); // pPointVelocity.add(tempVector); } }