public void packInverseTransformFromBasePelvisToWheel(RigidBodyTransform transformToPack) { packTransformFromBasePelvisToWheel(transformToPack); transformToPack.invert(); }
public void setTransformFromControlFrameToEndEffectorBodyFixedFrame(RigidBodyTransform transformFromControlFrameToEndEffectorBodyFixedFrame) { endEffectorToControlFrameOffset.set(transformFromControlFrameToEndEffectorBodyFixedFrame); endEffectorToControlFrameOffset.invert(); }
private void ensureForwardTransformUpToDate() { if (forwardTransformIsOutOfDate) { forwardTransform.set(backwardTransform); forwardTransform.invert(); forwardTransformIsOutOfDate = false; } }
private void ensureBackwardTransformUpToDate() { if (backwardTransformIsOutOfDate) { backwardTransform.set(forwardTransform); backwardTransform.invert(); backwardTransformIsOutOfDate = false; } }
/** * Compute the inverse of the RigidBodyTransform passed in as an * argument exploiting the orthogonality of the rotation matrix * and store the result in this. * @param transform */ public void invert(RigidBodyTransform transform) { if (transform != this) { set(transform); } invert(); }
/** * 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.invert(transformToWorld); if(updateInSimulationThread) { updateRootTransform(); } } }
/** * Compute the inverse of this, first checking if the scales are near unity * so an efficient inverse can be used. */ @Override public void invert() { computeScale(); if (Math.abs(scale1 - 1.0) < 1e-6 && Math.abs(scale2 - 1.0) < 1e-6 && Math.abs(scale3 - 1.0) < 1e-6) { super.invert(); } else { invertAffine(); } }
private void update() { frame.getTransformToDesiredFrame(transform, ReferenceFrame.getWorldFrame()); transform.invert(); }
public void setControlFrameFixedInEndEffector(ReferenceFrame controlFrame) { controlFrame.getTransformToDesiredFrame(endEffectorToControlFrameOffset, jacobian.getEndEffectorFrame()); endEffectorToControlFrameOffset.invert(); }
public static ReferenceFrame constructFrameWithUnchangingTransformFromParent(String frameName, ReferenceFrame parentFrame, RigidBodyTransform transformFromParent) { RigidBodyTransform transformToParent = new RigidBodyTransform(transformFromParent); transformToParent.invert(); return constructFrameWithUnchangingTransformToParent(frameName, parentFrame, transformToParent); }
public static Transform getInverse(Transform transform) { RigidBodyTransform transform3D = jmeTransformToTransform3D(transform); transform3D.invert(); Transform ret = j3dTransform3DToJMETransform(transform3D); return ret; }
/** * 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.invert(fromLocalToWorldTransform); concaveHullsVertices = new ArrayList<>(); convexPolygons = new ArrayList<>(); convexPolygons.add(convexPolygon); boundingBox3dInWorld.setEpsilonToGrow(DEFAULT_BOUNDING_BOX_EPSILON); updateBoundingBox(); updateConvexHull(); }
public static RigidBodyTransform getTransformFromA2toA1(RigidBodyTransform transformFromBtoA1, RigidBodyTransform transformFromBtoA2) { RigidBodyTransform temp = new RigidBodyTransform(transformFromBtoA2); temp.invert(); RigidBodyTransform ret = new RigidBodyTransform(transformFromBtoA1); ret.multiply(temp); return ret; }
public void getTransformToParent(RigidBodyTransform transformToParentToPack) { if (parentNode == null) { transformToParentToPack.setIdentity(); } parentNode.getSoleTransform(transformToParentToPack); transformToParentToPack.invert(); transformToParentToPack.multiply(transformToParentToPack, soleTransform); }
public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
private void setupRotationTransforms(int i) { tempTransformFromCurrentToRotary.setIdentity(); tempTransformFromCurrentToRotary.setRotationRollAndZeroTranslation(indexToAngle(i)); tempInverseTransformFromCurrentToRotary.invert(tempTransformFromCurrentToRotary); }
public static Transform invert(Transform transform) { RigidBodyTransform transform3D = jmeTransformToTransform3D(transform); transform3D.invert(); return transform.set( j3dTransform3DToJMETransform(transform3D ) ); }
/** * 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) { fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.invert(fromLocalToWorldTransform); concaveHullsVertices = new ArrayList<>(); convexPolygons = planarRegionConvexPolygons; boundingBox3dInWorld.setEpsilonToGrow(DEFAULT_BOUNDING_BOX_EPSILON); updateBoundingBox(); updateConvexHull(); }
/** * 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, List<Point2d[]> concaveHullsVertices, List<ConvexPolygon2d> planarRegionConvexPolygons) { fromLocalToWorldTransform.set(transformToWorld); fromWorldToLocalTransform.invert(fromLocalToWorldTransform); this.concaveHullsVertices = concaveHullsVertices; convexPolygons = planarRegionConvexPolygons; boundingBox3dInWorld.setEpsilonToGrow(DEFAULT_BOUNDING_BOX_EPSILON); updateBoundingBox(); updateConvexHull(); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.invert(transformShoulderToEndEffector); transformEndEffectorToDesired.multiply(transformEndEffectorToShoulder, transformShoulderToDesired); }