private void checkBodyFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBodyFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBodyFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { unitSuccessorTwist.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); unitSuccessorTwist.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); this.tau = unitSuccessorTwist.dot(jointWrench); // cheating a little bit; tau = J^T * wrench. J maps joint velocities to joint twists. // the unit twist is actually exactly the same as J, except that its entries have different dimensions. // we disregard dimensions and just use .dot(.) for efficiency }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
public void setJointTwist(Twist jointTwist) { this.jointTwist.checkReferenceFramesMatch(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); this.jointTwist.setAngularPartY(jointTwist.getAngularPartY()); this.jointTwist.setLinearPartX(jointTwist.getLinearPartX()); this.jointTwist.setLinearPartZ(jointTwist.getLinearPartZ()); }
public void getLinearAccelerationFromOriginAcceleration(Twist twistOfBodyWithRespectToBase, FrameVector linearAccelerationToPack) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearAccelerationToPack.setToZero(bodyFrame); linearAccelerationToPack.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearAccelerationToPack.add(linearPart); }
/** * The implementation for this method generates garbage and is wrong. * Do not use it or fix it. * * The implementation should be something like this: * <p> {@code RigidBodyTransform inverseTransformToRoot = afterJointFrame.getInverseTransformToRoot();} * <p> {@code inverseTransformToRoot.transform(linearVelocityInWorld);} * <p> {@code jointTwist.setLinearPart(linearVelocityInWorld);} * * Sylvain * * @deprecated * @param linearVelocityInWorld */ public void setLinearVelocityInWorld(Vector3d linearVelocityInWorld) { Twist newTwist = new Twist(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), ReferenceFrame.getWorldFrame()); newTwist.setLinearPart(linearVelocityInWorld); newTwist.changeFrame(jointTwist.getExpressedInFrame()); newTwist.setAngularPart(jointTwist.getAngularPart()); jointTwist.set(newTwist); }
public void setBasedOnOriginAcceleration(FrameVector angularAcceleration, FrameVector originAcceleration, Twist twistOfBodyWithRespectToBase) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); angularAcceleration.changeFrame(bodyFrame); angularPart.set(angularAcceleration.getVector()); originAcceleration.changeFrame(bodyFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearPart.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearPart.sub(originAcceleration.getVector(), linearPart); }
expressedInFrame.checkReferenceFrameMatch(twistOfCurrentWithRespectToNew.getBodyFrame()); newReferenceFrame.checkReferenceFrameMatch(twistOfCurrentWithRespectToNew.getBaseFrame()); bodyFrame.checkReferenceFrameMatch(twistOfBodyWithRespectToBase.getBodyFrame()); baseFrame.checkReferenceFrameMatch(twistOfBodyWithRespectToBase.getBaseFrame());
zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack);
public static void integrate(Matrix3d rotationToPack, Tuple3d positionToPack, double dt, Twist twist) { twist.changeFrame(twist.getBodyFrame()); Vector3d dPosition = new Vector3d(); twist.getLinearPart(dPosition); // velocity in body frame rotationToPack.transform(dPosition); // velocity in base frame dPosition.scale(dt); // translation in base frame positionToPack.add(dPosition); Vector3d axis = new Vector3d(); twist.getAngularPart(axis); axis.scale(dt); double angle = axis.length(); if (angle > 0.0) axis.normalize(); else axis.set(1.0, 0.0, 0.0); AxisAngle4d axisAngle = new AxisAngle4d(axis, angle); Matrix3d dRotation = new Matrix3d(); dRotation.set(axisAngle); rotationToPack.mul(dRotation); }
/** * Computes the kinetic co-energy of the rigid body to which this inertia belongs * twistTranspose * Inertia * twist * * See Duindam, Port-Based Modeling and Control for Efficient Bipedal Walking Robots, page 40, eq. (2.56) * http://sites.google.com/site/vincentduindam/publications * * @param twist * @return */ public double computeKineticCoEnergy(Twist twist) { this.expressedInframe.checkReferenceFrameMatch(twist.getExpressedInFrame()); this.bodyFrame.checkReferenceFrameMatch(twist.getBodyFrame()); twist.getBaseFrame().checkIsWorldFrame(); double ret = 0.0; tempVector.set(twist.getAngularPart()); // omega massMomentOfInertiaPart.transform(tempVector); // J * omega ret += twist.getAngularPart().dot(tempVector); // omega . J * omega tempVector.set(twist.getAngularPart()); // omega tempVector.cross(crossPart, tempVector); // c x omega ret += 2.0 * twist.getLinearPart().dot(tempVector); // omega . J * omega + 2 * v . (c x omega) ret += mass * twist.getLinearPart().dot(twist.getLinearPart()); // omega . J * omega + 2 * v . (c x omega) + m * v . v return ret; }
/** * Packs the linear acceleration of a point that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame * * */ public void getAccelerationOfPointFixedInBodyFrame(Twist twist, FramePoint pointFixedInBodyFrame, FrameVector frameVectorToPack) { expressedInFrame.checkReferenceFrameMatch(baseFrame); pointFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); expressedInFrame.checkReferenceFrameMatch(twist.getExpressedInFrame()); bodyFrame.checkReferenceFrameMatch(twist.getBodyFrame()); baseFrame.checkReferenceFrameMatch(twist.getBaseFrame()); frameVectorToPack.setToZero(expressedInFrame); Vector3d vectorToPack = frameVectorToPack.getVector(); tempVector.set(pointFixedInBodyFrame.getPoint()); vectorToPack.cross(angularPart, tempVector); vectorToPack.add(linearPart); tempVector.set(pointFixedInBodyFrame.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempVector.add(twist.getLinearPart()); tempVector.cross(twist.getAngularPart(), tempVector); vectorToPack.add(tempVector); }
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); }
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }