public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getLinearPart(frameVectorToPack); } }
public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getAngularPart(frameVectorToPack); } }
private void updateFootLinearVelocities() { for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); twistCalculator.getTwistOfBody(footTwist, foot); footTwist.getLinearPart(footLinearVelocity); currentFootLinearVelocities.get(foot).set(footLinearVelocity.length()); } }
private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBody body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); twistCalculator.getTwistOfBody(tempTwist, body); if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(tempAcceleration, body); body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); } }
/** * Computes and packs the angular velocity of the given {@code body} with respect to * {@code inertialFrame}. * <p> * The result will be the angular velocity of {@code body.getBodyFixedFrame()} with respect to * {@code inertialFrame} expressed in {@code body.getBodyFixedFrame()}. * </p> * * @param body the rigid-body to compute the angular velocity of. * @param angularVelocityToPack the angular velocity of the given {@code body}. Modified. */ public void getAngularVelocityOfBody(RigidBodyBasics body, FrameVector3D angularVelocityToPack) { getTwistOfBody(body, twistForGetAngularVelocityOfBody); angularVelocityToPack.setIncludingFrame(twistForGetAngularVelocityOfBody.getAngularPart()); }
public static void main(String[] args) { Random random = new Random(); int numberOfJoints = 5; List<RevoluteJoint> randomChainRobot = MultiBodySystemRandomTools.nextRevoluteJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, randomChainRobot.get(0).getPredecessor()); Twist dummyTwist = new Twist(); while (true) { twistCalculator.compute(); for (int i = 0; i < 100; i++) twistCalculator.getTwistOfBody(randomChainRobot.get(random.nextInt(numberOfJoints)).getSuccessor(), dummyTwist); } } }
public void startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(measurementFrame); twist.getAngularPart(angularVelocity); yoFrameVectorPerfect.set(angularVelocity); corrupt(angularVelocity); yoFrameVectorNoisy.set(angularVelocity); angularVelocityOutputPort.setData(angularVelocity); }
/** * Computes and packs the linear velocity of a point defined by {@code bodyFixedPoint} that is * fixed to the given {@code body}. * <p> * The result will be the linear velocity of the {@code bodyFixedPoint} with respect to the * {@code inertialFrame}. The vector is expressed in {@code inertialFrame}. * </p> * * @param body the rigid-body to which {@code bodyFixedPoint} is attached to. * @param bodyFixedPoint the coordinates of the point attached to {@code body} that linear * velocity is to be computed. Not modified. * @param linearVelocityToPack the linear velocity of the body fixed point with respect to the * inertial frame. Modified. */ public void getLinearVelocityOfBodyFixedPoint(RigidBodyBasics body, FramePoint3D bodyFixedPoint, FrameVector3D linearVelocityToPack) { FramePoint3D localPoint = pointForGetLinearVelocityOfBodyFixedPoint; Twist localTwist = twistForGetLinearVelocityOfBodyFixedPoint; getTwistOfBody(body, localTwist); ReferenceFrame baseFrame = localTwist.getBaseFrame(); localPoint.setIncludingFrame(bodyFixedPoint); localTwist.changeFrame(baseFrame); localPoint.changeFrame(baseFrame); localTwist.getLinearVelocityAt(localPoint, linearVelocityToPack); }
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); } }
public void computeAndPack(Momentum momentum) { momentum.setAngularPart(zero); momentum.setLinearPart(zero); for (RigidBody rigidBody : rigidBodiesInOrders) { RigidBodyInertia inertia = rigidBody.getInertia(); twistCalculator.getTwistOfBody(tempTwist, rigidBody); tempMomentum.compute(inertia, tempTwist); tempMomentum.changeFrame(momentum.getExpressedInFrame()); momentum.add(tempMomentum); } } }
private void computeRpd(FrameVector rPdToPack, TwistCalculator twistCalculator, FramePoint rP, FrameVector rd) { // T_{p}^{p,w} twistCalculator.getTwistOfBody(twistOfEstimationLink, estimationLink); twistOfEstimationLink.changeFrame(estimationFrame); // \dot{r}^{p} = R_{w}^{p} \dot{r} - \tilde{\omega}r^{p} - v_{p}^{p,w} rPdToPack.setIncludingFrame(rd); rPdToPack.changeFrame(estimationFrame); twistOfEstimationLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); rPdToPack.sub(tempFrameVector); twistOfEstimationLink.getLinearPart(tempFrameVector); rPdToPack.sub(tempFrameVector); }
private void computeSuccessorAcceleration(InverseDynamicsJoint joint) { RigidBody predecessor = joint.getPredecessor(); RigidBody successor = joint.getSuccessor(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); joint.getPredecessorTwist(tempJointTwist); if (!doVelocityTerms) tempJointTwist.setToZero(); if (useDesireds) joint.getDesiredSuccessorAcceleration(tempJointAcceleration); else joint.getSuccessorAcceleration(tempJointAcceleration); if (!doAccelerationTerms) tempJointAcceleration.setToZero(); twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor); if (!doVelocityTerms) tempTwistFromWorld.setToZero(); SpatialAccelerationVector successorAcceleration = accelerations.get(successor); successorAcceleration.set(accelerations.get(predecessor)); successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld); successorAcceleration.add(tempJointAcceleration); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithPrismaticChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<PrismaticJoint> joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
twistOfCurrentWithRespectToNew.changeFrame(base.getBodyFixedFrame()); twistCalculator.getTwistOfBody(twistOfBodyWithRespectToBase, base);
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 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); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testWithChainComposedOfPrismaticJoints() throws Exception { Random random = new Random(234234L); int numberOfJoints = 20; List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -10.0, 10.0, prismaticJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, prismaticJoints); twistCalculator.compute(); FrameVector3D cumulatedLinearVelocity = new FrameVector3D(worldFrame); for (PrismaticJoint joint : prismaticJoints) { RigidBodyBasics body = joint.getSuccessor(); Twist actualTwist = new Twist(); twistCalculator.getTwistOfBody(body, actualTwist); ReferenceFrame bodyFrame = body.getBodyFixedFrame(); Twist expectedTwist = new Twist(bodyFrame, worldFrame, bodyFrame); FrameVector3D jointAxis = new FrameVector3D(joint.getJointAxis()); cumulatedLinearVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedLinearVelocity.scaleAdd(joint.getQd(), jointAxis, cumulatedLinearVelocity); cumulatedLinearVelocity.changeFrame(bodyFrame); expectedTwist.getLinearPart().set(cumulatedLinearVelocity); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12); } } }
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
@Override public void read() { twistCalculator.compute(); spatialAccelerationCalculator.reset(); twistCalculator.getTwistOfBody(rigidBody, twist); // Twist of bodyCoM and not IMU! spatialAcceleration.setIncludingFrame(spatialAccelerationCalculator.getAccelerationOfBody(rigidBody)); spatialAcceleration.changeFrame(worldFrame, twist, twist); updatePerfectOrientation(); updatePerfectAngularVelocity(); updatePerfectAcceleration(); updatePerfectCompass(); simulateIMU(); orientation.set(rotationMatrix.getMatrix3d()); acceleration.set(accelX.getDoubleValue(), accelY.getDoubleValue(), accelZ.getDoubleValue()); angularVelocity.set(gyroX.getDoubleValue(), gyroY.getDoubleValue(), gyroZ.getDoubleValue()); compass.set(compassX.getDoubleValue(), compassY.getDoubleValue(), compassZ.getDoubleValue()); rawSensors.setOrientation(orientation, imuIndex); rawSensors.setAcceleration(new Vector3D(acceleration), imuIndex); rawSensors.setAngularVelocity(angularVelocity, imuIndex); rawSensors.setCompass(compass, imuIndex); }