protected void putYoValuesIntoTwist() { twist.setToZero(bodyFrame, baseFrame, expressedInFrame); twist.setLinearPart(linearPart.getFrameTuple()); twist.setAngularPart(angularPart.getFrameTuple()); }
public static Twist generateRandomTwist(Random random, ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, double angularVelocityMagnitude, double linearVelocityMagnitude) { Twist randomTwist = new Twist(bodyFrame, baseFrame, expressedInFrame); randomTwist.setLinearPart(RandomTools.generateRandomVector(random, linearVelocityMagnitude)); randomTwist.setAngularPart(RandomTools.generateRandomVector(random, angularVelocityMagnitude)); return randomTwist; }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { int index = rowStart; double qdRot = matrix.get(index++, 0); double xd = matrix.get(index++, 0); double zd = matrix.get(index++, 0); jointTwist.setToZero(); jointTwist.setAngularPartY(qdRot); jointTwist.setLinearPart(xd, 0.0, zd); }
public void setLinearVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredLinearVelocity.getReferenceFrame()); spatialVelocity.setLinearPart(desiredLinearVelocity.getVector()); spatialVelocity.changeFrame(bodyFrame); setSelectionMatrixForLinearControl(); }
public static void setRandomVelocity(FloatingInverseDynamicsJoint rootJoint, Random random) { Twist jointTwist = new Twist(); rootJoint.getJointTwist(jointTwist); jointTwist.setAngularPart(RandomTools.generateRandomVector(random)); jointTwist.setLinearPart(RandomTools.generateRandomVector(random)); rootJoint.setJointTwist(jointTwist); }
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 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(); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); setPosition(sixDoFOriginalJoint.jointTranslation); setRotation(sixDoFOriginalJoint.jointRotation); jointTwist.setAngularPart(sixDoFOriginalJoint.jointTwist.getAngularPart()); jointTwist.setLinearPart(sixDoFOriginalJoint.jointTwist.getLinearPart()); jointAcceleration.setAngularPart(sixDoFOriginalJoint.jointAcceleration.getAngularPart()); jointAcceleration.setLinearPart(sixDoFOriginalJoint.jointAcceleration.getLinearPart()); }
/** * 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()); }
/** * 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); }
/** * Compute the center of mass velocity given a centroidal momentum matrix. * Just takes linear portion of centroidal momentum matrix(bottom three rows), * multiplies by vector of joint velocities, and divide by the robots mass. */ private void computeCoMVelocity(DenseMatrix64F centroidalMomentumMatrixDense) { comVelocityVector.set(0, 0, 0); for (int i = 0; i < centroidalMomentumMatrixDense.numCols; i++) { comVelocityVector.setX(comVelocityVector.getX() + centroidalMomentumMatrixDense.get(3, i) * v.get(i)); comVelocityVector.setY(comVelocityVector.getY() + centroidalMomentumMatrixDense.get(4, i) * v.get(i)); comVelocityVector.setZ(comVelocityVector.getZ() + centroidalMomentumMatrixDense.get(5, i) * v.get(i)); } if (robotMass != 0.0) { comVelocityVector.scale(1 / robotMass); } else { throw new RuntimeException("Your robot has zero mass"); } comTwist.setLinearPart(comVelocityVector); //Velocity of CoM w.r.t rootBody expressed in CoM frame. Angular part is zero. }
/** * 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()); }
comTwist.setLinearPart(tempVector);