private void checkExpressedInFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkExpressedInFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkExpressedInFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); currentTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); }
protected void setMotionSubspace(Twist unitTwist) { ArrayList<Twist> unitTwists = new ArrayList<Twist>(); unitTwists.add(unitTwist); this.motionSubspace = new GeometricJacobian(this, unitTwists, unitTwist.getExpressedInFrame()); this.motionSubspace.compute(); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart); }
public void compute(FrameVector outputToPack, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration, RigidBody base) { // using twists is a bit overkill; optimize if needed. twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); currentAngularVelocity.setToZero(endEffectorTwist.getExpressedInFrame()); endEffectorTwist.getAngularPart(currentAngularVelocity); desiredAngularVelocity.changeFrame(currentAngularVelocity.getReferenceFrame()); feedForwardAngularAcceleration.changeFrame(endEffectorTwist.getExpressedInFrame()); axisAngleOrientationController.compute(outputToPack, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAcceleration); }
protected void getYoValuesFromTwist() { linearPart.set(twist.getExpressedInFrame(), twist.getLinearPartX(), twist.getLinearPartY(), twist.getLinearPartZ()); angularPart.set(twist.getExpressedInFrame(), twist.getAngularPartX(), twist.getAngularPartY(), twist.getAngularPartZ()); }
public double computeKineticCoEnergy(Twist twist) { getExpressedInFrame().checkReferenceFrameMatch(twist.getExpressedInFrame()); double ret = linearPart.dot(twist.linearPart) + angularPart.dot(twist.angularPart); return ret; }
private void updateKinematicsNewTwist() { rootJoint.getJointTwist(tempRootBodyTwist); rootJointLinearVelocityNewTwist.getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.changeFrame(tempRootBodyTwist.getExpressedInFrame()); tempRootBodyTwist.setLinearPart(tempFrameVector); rootJoint.setJointTwist(tempRootBodyTwist); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); Twist footTwistInWorld = footTwistsInWorld.get(foot); YoFrameVector footVelocityInWorld = footVelocitiesInWorld.get(foot); twistCalculator.getTwistOfBody(footTwistInWorld, foot); footTwistInWorld.changeBodyFrameNoRelativeTwist(soleFrames.get(foot)); footTwistInWorld.changeFrame(soleFrames.get(foot)); this.copsFilteredInFootFrame.get(foot).getFrameTuple2dIncludingFrame(tempCoP2d); tempCoP.setXYIncludingFrame(tempCoP2d); footTwistInWorld.changeFrame(footTwistInWorld.getBaseFrame()); tempCoP.changeFrame(footTwistInWorld.getExpressedInFrame()); footTwistInWorld.getLinearVelocityOfPointFixedInBodyFrame(tempFrameVector, tempCoP); tempFrameVector.changeFrame(worldFrame); footVelocityInWorld.set(tempFrameVector); } }
@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()); }
private void updateRootJoint() { yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); tempVelocity.setIncludingFrame(rootJointVelocity); rootJoint.getJointTwist(rootJointTwist); tempVelocity.changeFrame(rootJointTwist.getExpressedInFrame()); rootJointTwist.setLinearPart(tempVelocity); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); }
tempTwist.changeFrame(twistToPack.getExpressedInFrame()); twistToPack.sub(tempTwist);
/** * 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); }
/** * Sets this momentum to inertia * twist */ public void compute(GeneralizedRigidBodyInertia inertia, Twist twist) { ReferenceFrame frame = inertia.getExpressedInFrame(); frame.checkReferenceFrameMatch(twist.getExpressedInFrame()); tempVector.set(twist.getAngularPart()); inertia.massMomentOfInertiaPart.transform(tempVector); // J * omegad setAngularPart(tempVector); if (!inertia.isCrossPartZero()) { tempVector.cross(twist.getLinearPart(), inertia.crossPart); addAngularPart(tempVector); // J * omegad - c x vd } tempVector.set(twist.getLinearPart()); tempVector.scale(inertia.getMass()); setLinearPart(tempVector); // m * vd if (!inertia.isCrossPartZero()) { tempVector.cross(inertia.crossPart, twist.getAngularPart()); addLinearPart(tempVector); // m * vd + c x omegad } this.expressedInFrame = frame; }
/** * 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] }