@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotation(frameOrientation); } }
/** * Sets the orientation of this shape. * <p> * This method does not affect the position of this shape. * </p> * * @param orientation the new orientation for this shape. Not modified. */ public final void setOrientation(Orientation3DReadOnly orientation) { shapePose.setRotation(orientation); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setTranslation(pelvisTranslationFromRobotConfigurationData); transformToParent.setRotation(pelvisOrientationFromRobotConfigurationData); } };
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); rigidBodyTransformToPack.setRotation(tempFrameOrientation); rigidBodyTransformToPack.setTranslation(position); }
public void packPose(RigidBodyTransform pose) { tempQuaternion.set(qx, qy, qz, qw); pose.setRotation(tempQuaternion); pose.setTranslation(xPosition, yPosition, zPosition); } }
private RigidBodyTransform[] createYawOnlyCorrectionTargets(Random random, int numTargets) { RigidBodyTransform[] targets = new RigidBodyTransform[numTargets]; Quaternion rot = new Quaternion(); for (int i = 0; i < 10; i++) { targets[i] = new RigidBodyTransform(); rot.setYawPitchRoll((random.nextDouble() * Math.PI * 2) - Math.PI, 0, 0); targets[i].setRotation(rot); } return targets; }
/** * Creates a transform that transforms to the given point and rotates to make the z axis align * with the normal vector. */ public static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint3D point, FrameVector3D zAxis) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotation(EuclidGeometryTools.axisAngleFromZUpToVector3D(zAxis)); ret.setTranslation(point); return ret; }
public static void tranformSe3IntoTransform3D(Se3_F64 from, RigidBodyTransform to) { DenseMatrix64F R = from.getR(); Vector3D_F64 T = from.getT(); RotationMatrix Rd = new RotationMatrix(R); to.setRotation(Rd); to.setTranslation(new Vector3D(T.x, T.y, T.z)); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { xAxis.set(positionToPointAt); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotation(rotationMatrix); } }
public void setFromAnklePose(FramePose3DReadOnly anklePose, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(anklePose.getOrientation()); tempTransform.setTranslation(anklePose.getPosition()); tempTransform.multiplyInvertOther(transformFromAnkleToSole); footstepPose.setIncludingFrame(anklePose.getReferenceFrame(), tempTransform); }
public static Affine createAffineFromQuaternionAndTuple(Quaternion32 quaternion, Tuple3DBasics translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotation(quaternion); transform.setTranslation(translation.getX(), translation.getY(), translation.getZ()); return convertRigidBodyTransformToAffine(transform); }
public void getAnklePose(FramePose3D poseToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); poseToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform); }
public static Affine createAffineFromQuaternionAndTuple(Quaternion quaternion, Tuple3DBasics translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotation(quaternion); transform.setTranslation(translation.getX(), translation.getY(), translation.getZ()); return convertRigidBodyTransformToAffine(transform); }
public void getAnkleOrientation(FrameQuaternion orientationToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); orientationToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getRotationMatrix()); }
public void getAnklePosition(FramePoint3D positionToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); positionToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getTranslationVector()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { yoFramePose.getOrientation().getQuaternion(rotation); transformToParent.setRotation(rotation); YoFramePoint3D yoFramePoint = yoFramePose.getPosition(); transformToParent.setTranslation(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleRotation() { FootstepNode nodeToSnap = new FootstepNode(0.0, 0.0); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setRotation(new AxisAngle(0.0, 1.0, 0.0, 0.25 * Math.PI)); doAFullFootholdTest(transformToWorld, nodeToSnap); }
private Footstep generateFootstepFromLocationAndOrientation(RobotSide robotSide, double[] positionArray, double[] orientationArray) { Footstep footstep = new Footstep(robotSide); Point3D position = new Point3D(positionArray); Quaternion orientation = new Quaternion(orientationArray); RigidBodyTransform anklePose = new RigidBodyTransform(); anklePose.setRotation(orientation); anklePose.setTranslation(position); FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), anklePose); footstep.setFromAnklePose(pose, transformsFromAnkleToSole.get(robotSide)); return footstep; }
public void getAnklePosition2d(FramePoint2D position2dToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); double x = tempTransform.getTranslationVector().getX(); double y = tempTransform.getTranslationVector().getY(); position2dToPack.setIncludingFrame(footstepPose.getReferenceFrame(), x, y); }
public static void packExtrapolatedTransform(RigidBodyTransform from, RigidBodyTransform to, double ratio, RigidBodyTransform toPack) { Point3D pointToPack = new Point3D(); RotationMatrix orientationToPack = new RotationMatrix(); packExtrapolatedPoint(from.getTranslationVector(), to.getTranslationVector(), ratio, pointToPack); packExtrapolatedOrienation(from.getRotationMatrix(), to.getRotationMatrix(), ratio, orientationToPack); toPack.setIdentity(); toPack.setTranslation(pointToPack); toPack.setRotation(orientationToPack); }