public void getEndEffectorCurrentAngularVelocity(FrameVector endEffectorAngularVelocityToPack) { currentTwist.getAngularVelocityInBaseFrame(endEffectorAngularVelocityToPack); }
/** * Packs the angular velocity of the body frame with respect to the base frame, expressed in the base frame. * The vector is computed by simply rotating the angular velocity part of this twist to base frame. */ public void getAngularVelocityInBaseFrame(FrameVector vectorToPack) { vectorToPack.setToZero(baseFrame); getAngularVelocityInBaseFrame(vectorToPack.getVector()); }
ReferenceFrame chestFrame = chest.getBodyFixedFrame(); FrameVector desiredChestAngularVelocity = new FrameVector(); desiredChestTwist.getAngularVelocityInBaseFrame(desiredChestAngularVelocity); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, chest); ReferenceFrame pelvisFrame = pelvis.getBodyFixedFrame(); FrameVector desiredPelvisAngularVelocity = new FrameVector(); desiredPelvisTwist.getAngularVelocityInBaseFrame(desiredPelvisAngularVelocity); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, pelvis);