/** * Set the transform from the root joint joint of the robot to the world, as known by the controller. If both * this and the simulation transform are set, all elements are adjusted for the difference. * * @param transformToWorld */ public void setControllerTransformToWorld(RigidBodyTransform transformToWorld) { this.controllerWorldToRootTransform.setAndInvert(transformToWorld); if(updateInSimulationThread) { updateRootTransform(); } }
public void set(RigidBodyTransform transformToWorld, List<ConvexPolygon2D> planarRegionConvexPolygons, int newRegionId) { fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.setAndInvert(fromLocalToWorldTransform); convexPolygons.clear(); convexPolygons.addAll(planarRegionConvexPolygons); updateBoundingBox(); updateConvexHull(); regionId = newRegionId; }
/** * Create a new planar region. * * @param transformToWorld transform from the region local coordinate system to world. * @param concaveHullVertices vertices of the concave hull of the region. * @param planarRegionConvexPolygons the list of convex polygon that represents the planar * region. Expressed in local coordinate system. */ public PlanarRegion(RigidBodyTransform transformToWorld, Point2D[] concaveHullVertices, List<ConvexPolygon2D> planarRegionConvexPolygons) { fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.setAndInvert(fromLocalToWorldTransform); this.concaveHullsVertices = concaveHullVertices; convexPolygons = planarRegionConvexPolygons; updateBoundingBox(); updateConvexHull(); }
public void setRegionProperties(int id, Tuple3DReadOnly origin, Tuple3DReadOnly normal) { regionId = id; regionOrigin.set(origin); regionNormal.set(normal); EuclidGeometryTools.axisAngleFromZUpToVector3D(regionNormal, regionOrientation); fromLocalToWorldTransform.set(regionOrientation, regionOrigin); fromWorldToLocalTransform.setAndInvert(fromLocalToWorldTransform); }
/** * Create a new planar region. * * @param transformToWorld transform from the region local coordinate system to world. * @param planarRegionConvexPolygons the list of convex polygon that represents the planar * region. Expressed in local coordinate system. */ public PlanarRegion(RigidBodyTransform transformToWorld, List<ConvexPolygon2D> planarRegionConvexPolygons) { PrintTools.warn("This constructor does not set the concave hull."); fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.setAndInvert(fromLocalToWorldTransform); concaveHullsVertices = new Point2D[0]; convexPolygons = planarRegionConvexPolygons; updateBoundingBox(); updateConvexHull(); }
/** * Create a new planar region. * * @param transformToWorld transform from the region local coordinate system to world. * @param convexPolygon a single convex polygon that represents the planar region. Expressed in * local coordinate system. */ public PlanarRegion(RigidBodyTransform transformToWorld, ConvexPolygon2D convexPolygon) { fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.setAndInvert(fromLocalToWorldTransform); concaveHullsVertices = new Point2D[convexPolygon.getNumberOfVertices()]; for (int i = 0; i < convexPolygon.getNumberOfVertices(); i++) { concaveHullsVertices[i] = new Point2D(convexPolygon.getVertex(i)); } convexPolygons = new ArrayList<>(); convexPolygons.add(convexPolygon); updateBoundingBox(); updateConvexHull(); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
/** * Transforms footPolygonInRegionFrame from planarRegion frame to the snapped footstep node frame * * @param planarRegion * @param footstepNode * @param snapTransform * @param footPolygonInRegionFrame */ public static void changeFromPlanarRegionToSoleFrame(PlanarRegion planarRegion, FootstepNode footstepNode, RigidBodyTransform snapTransform, ConvexPolygon2D footPolygonInRegionFrame) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); planarRegion.getTransformToWorld(regionToWorld); RigidBodyTransform soleTransform = new RigidBodyTransform(); FootstepNodeTools.getSnappedNodeTransform(footstepNode, snapTransform, soleTransform); RigidBodyTransform regionToSole = new RigidBodyTransform(); regionToSole.setAndInvert(soleTransform); regionToSole.multiply(regionToWorld); footPolygonInRegionFrame.applyTransform(regionToSole, false); }
transformToPack.setAndInvert(desiredFrame.getTransformToRoot()); transformToPack.setAndInvert(desiredFrame.transformToParent); transformToPack.setAndInvert(desiredFrame.transformToParent); transformToPack.multiply(transformToParent); transformToPack.setAndInvert(desiredFrame.transformToParent); if (!desiredFrame.parentFrame.isRootFrame()) // If it is the root, then desiredFrame.parentFrame.transformToParent is identity. transformToPack.multiplyInvertOther(desiredFrame.parentFrame.transformToParent); transformToPack.setAndInvert(desiredFrame.getTransformToRoot()); transformToPack.multiply(getTransformToRoot());
this.totalWork[currentSample] = totalWorkVariable.getDoubleValue(); inverse.setAndInvert(rootBody.getChildrenJoints().get(0).getFrameAfterJoint().getTransformToRoot()); inverse.getTranslation(position); this.xPosition[currentSample] = position.getX();
@Test public void testGetTransformBetweenFrames() { Random random = new Random(1776L); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] treeFrame = EuclidFrameRandomTools.nextReferenceFrameTree(random); ReferenceFrame frame1 = treeFrame[random.nextInt(treeFrame.length)]; ReferenceFrame frame2 = treeFrame[random.nextInt(treeFrame.length)]; RigidBodyTransform transformOne = frame1.getTransformToDesiredFrame(frame2); RigidBodyTransform transformTwo = frame2.getTransformToDesiredFrame(frame1); transformTwo.invert(); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformOne, transformTwo, EPSILON); RigidBodyTransform transformThree = new RigidBodyTransform(); if (frame2.getTransformToRoot() != null) transformThree.setAndInvert(frame2.getTransformToRoot()); if (frame1.getTransformToRoot() != null) transformThree.multiply(frame1.getTransformToRoot()); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformOne, transformThree, EPSILON); RigidBodyTransform transformFour = new RigidBodyTransform(); transformFour.set(worldFrame.getTransformToDesiredFrame(frame2)); transformFour.multiply(frame1.getTransformToDesiredFrame(worldFrame)); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformOne, transformFour, EPSILON); ReferenceFrame frame3 = treeFrame[random.nextInt(treeFrame.length)]; RigidBodyTransform transformFive = new RigidBodyTransform(); transformFive.set(frame3.getTransformToDesiredFrame(frame2)); transformFive.multiply(frame1.getTransformToDesiredFrame(frame3)); EuclidCoreTestTools.assertRigidBodyTransformEquals(transformOne, transformFive, EPSILON); } }
@Before public void setUp() throws Exception { transformIMUToJoint.setRotation(jointToIMURotation); transformIMUToJoint.setTranslation(jointToIMUOffset); transformJointToIMU.setAndInvert(transformIMUToJoint); imuFrame = fullRobotModel.createOffsetFrame(fullRobotModel.getBodyLink().getParentJoint(), transformIMUToJoint, "imuFrame"); Vector3D linearAcceleration = new Vector3D(0.0, 0.0, GRAVITY); Vector3D angularAcceleration = new Vector3D(); ReferenceFrame rootBodyFrame = fullRobotModel.getElevatorFrame(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBodyFrame, ReferenceFrame.getWorldFrame(), rootBodyFrame, angularAcceleration, linearAcceleration); simulatedIMURawSensorReader = new PerfectSimulatedIMURawSensorReader(rawSensors, IMU_INDEX, rigidBody, imuFrame, fullRobotModel.getElevator(), rootAcceleration); simulatedIMURawSensorReader.initialize(); }
root1.getTransformToWorld(transformToWorld); RigidBodyTransform transformToBody = new RigidBodyTransform(); transformToBody.setAndInvert(transformToWorld); transformToBody.transform(force); Vector3D moment = new Vector3D();
private void computeError(RigidBodyTransform desiredTransform) { jacobian.compute(); jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorRotationVector.scale(0.2); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); }
actualTransform.setAndInvert(expectedTransform); expectedTransform.invert(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expectedTransform, actualTransform, EPS);
private void computeError(RigidBodyTransform desiredTransform) { /* * B is base E is end effector D is desired end effector * * H^D_E = H^D_B * H^B_E = (H^B_D)^-1 * H^B_E * * convert rotation matrix part to rotation vector */ jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, error); errorTranslationVector.get(3, error); errorScalar = NormOps.normP2(error); assert (exponentialCoordinatesOK()); }
jointFrameToModelFrame.setAndInvert(modelFrameToJointFrame);
parentJointToModel.setAndInvert(modelToParentJoint);
expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1); expected.multiply(t2); actual.set(t1); expected.setAndInvert(t1);
RigidBodyTransform t2 = new RigidBodyTransform(new Quaternion(), EuclidCoreRandomTools.nextVector3D(random)); RigidBodyTransform t2Inverse = new RigidBodyTransform(); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse); t2Inverse.setAndInvert(t2); expected.set(t1); expected.multiply(t2Inverse);