private void checkBaseFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBaseFrame().checkReferenceFrameMatch(currentTwist.getBaseFrame()); }
private void checkBaseFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBaseFrame().checkReferenceFrameMatch(currentTwist.getBaseFrame()); }
private void checkBaseFrames(Twist desiredTwist, Twist currentTwist) { desiredTwist.getBaseFrame().checkReferenceFrameMatch(currentTwist.getBaseFrame()); }
private void checkBaseFrames(Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkBaseFrames(desiredTwist, currentTwist); desiredTwist.getBaseFrame().checkReferenceFrameMatch(feedForwardAcceleration.getBaseFrame()); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart); }
public void startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(twist.getBaseFrame()); tempPointToMeasureVelocityOf.setIncludingFrame(pointToMeasureVelocityOf); tempPointToMeasureVelocityOf.changeFrame(twist.getBaseFrame()); twist.getLinearVelocityOfPointFixedInBodyFrame(pointVelocityFrameVector, tempPointToMeasureVelocityOf); pointVelocityFrameVector.changeFrame(worldFrame); pointVelocityFrameVector.get(pointVelocity); yoFrameVectorPerfect.set(pointVelocity); corrupt(pointVelocity); yoFrameVectorNoisy.set(pointVelocity); pointVelocityOutputPort.setData(pointVelocity); }
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); } }
private void computeVelocityOfStationaryPoint(FrameVector stationaryPointVelocityToPack) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody stationaryPointLink = estimatorRigidBodyToIndexMap.getRigidBodyByName(pointVelocityMeasurementInputPort.getData().getRigidBodyName()); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); try { twistCalculator.getTwistOfBody(tempTwist, stationaryPointLink); } catch(Exception e) { e.printStackTrace(); } tempTwist.changeFrame(tempTwist.getBaseFrame()); ReferenceFrame referenceFrame = referenceFrameNameMap.getFrameByName(pointVelocityMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointVelocityMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(tempTwist.getBaseFrame()); tempTwist.getLinearVelocityOfPointFixedInBodyFrame(stationaryPointVelocityToPack, tempFramePoint); }
@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); }
/** * Computes using Twists, ignores linear part */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); }
/** * 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); }
/** * Computes linear portion of twist to pack */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
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); }
/** * This method provides a twist feedback controller, intended for spatial velocity control. * @param twistToPack twist to return * @param desiredPose desired pose that we want to achieve. * @param desiredTwist feed forward twist from a reference trajectory */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
/** * 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] }