@Override public Quat4d initialValue() { return new Quat4d(); } };
public SO3TrajectoryPointMessage(SO3TrajectoryPointMessage trajectoryPoint) { time = trajectoryPoint.time; if (trajectoryPoint.orientation != null) orientation = new Quat4d(trajectoryPoint.orientation); if (trajectoryPoint.angularVelocity != null) angularVelocity = new Vector3d(trajectoryPoint.angularVelocity); }
/** * Multiplies this quaternion by the inverse of quaternion q1 and places * the value into this quaternion. The value of the argument quaternion * is preserved (this = this * q^-1). * @param q1 the other quaternion */ public final void mulInverse(Quat4d q1) { Quat4d tempQuat = new Quat4d(q1); tempQuat.inverse(); this.mul(tempQuat); }
public static Quat4d generateRandomQuaternion(Random random, double minMaxAngleRange) { AxisAngle4d orientation = generateRandomRotation(random, minMaxAngleRange); Quat4d quat = new Quat4d(); quat.set(orientation); return quat; }
public FootstepPlanState(double x, double y, double theta, RobotSide side) { footstepData.location = new Point3d(x, y, 0); footstepData.orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(theta, 0, 0, footstepData.orientation); footstepData.robotSide = side; this.theta = theta; }
public static void packFrameOrientationInJMEQuaternion(FrameOrientation original, Quaternion target) { Quat4d quat4d = new Quat4d(); packVectMathQuat4dInJMEQuaternion(original.getQuaternion(), target); }
public ChestOrientationTask(E stateEnum, FrameOrientation desiredChestOrientation, ChestTrajectoryBehavior chestOrientationBehavior, double trajectoryTime) { super(stateEnum, chestOrientationBehavior); this.chestOrientationBehavior = chestOrientationBehavior; Quat4d chestOrientation = new Quat4d(); desiredChestOrientation.getQuaternion(chestOrientation); chestOrientationPacket = new ChestTrajectoryMessage(trajectoryTime, chestOrientation); }
@Override public void applyTransform(RigidBodyTransform transform) { if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(angularVelocity); }
public HandstepPacket(HandstepPacket handstepPacket) { this.robotSide = handstepPacket.robotSide; this.location = new Point3d(handstepPacket.location); this.orientation = new Quat4d(handstepPacket.orientation); this.surfaceNormal = new Vector3d(handstepPacket.surfaceNormal); RotationTools.checkQuaternionNormalized(this.orientation); }
public TorusPosePacket(Random random) { Point3d point = new Point3d(); Quat4d quat = new Quat4d(); point.set(RandomTools.generateRandomPoint(random, 0.288, 0.288, 0.288)); // magic numbers so point will not exceed XYZ_MIN / MAX in TorusPosePacketSerializer quat.set(RandomTools.generateRandomRotation(random)); this.orientation = quat; this.position = point; this.fingerHoleRadius = random.nextDouble(); } }
public Quat4d getQuaternion(int imuIndex) { return new Quat4d(p_qx.getDoubleValue(), p_qy.getDoubleValue(), p_qz.getDoubleValue(), p_qs.getDoubleValue()); }
private FramePose generateDebrisPose(Point3d positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose debrisPose = new FramePose(robotInitialPoseReferenceFrame); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(debrisYaw, debrisPitch, debrisRoll, orientation); debrisPose.setPose(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; }
@Override public void applyTransform(RigidBodyTransform transform) { transform.transform(position); if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(linearVelocity); transform.transform(angularVelocity); }
public VehiclePosePacket(Random random) { Point3d point = new Point3d(); Quat4d quat = new Quat4d(); point.set(RandomTools.generateRandomPoint(random, 0.288, 0.288, 0.288)); // magic numbers so point will not exceed XYZ_MIN / MAX in PelvisOrientationPacketSerializer quat.set(RandomTools.generateRandomRotation(random)); this.position = point; this.orientation = quat; } }
public static FramePose setupStanceFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startStanceFootPose = new FramePose(worldFrame, new Point3d(0.0, 0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.LEFT).setPoseAndUpdate(startStanceFootPose); soleFrames.get(RobotSide.LEFT).update(); Point3d stanceAnklePosition = new Point3d(); startStanceFootPose.getPosition(stanceAnklePosition); sixDoFJoints.get(RobotSide.LEFT).setPosition(stanceAnklePosition); return startStanceFootPose; }
public VehiclePosePacket(RigidBodyTransform transformFromVehicleToWorld) { Matrix3d rotationMatrix = new Matrix3d(); transformFromVehicleToWorld.getRotation(rotationMatrix); orientation = new Quat4d(); RotationTools.convertMatrixToQuaternion(rotationMatrix, orientation); Vector3d translation = new Vector3d(); transformFromVehicleToWorld.getTranslation(translation); position = new Point3d(translation); }
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quat4d desiredQuaternion = new Quat4d(); FrameOrientation desiredOrientation = new FrameOrientation(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredQuaternion); ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredQuaternion); output.setChestTrajectoryMessage(chestTrajectoryMessage); }
public static RigidBodyTransform jmeTransformToTransform3D(Transform jmeTransform) { Quaternion jmeQuat = jmeTransform.getRotation(); Vector3f jmeVect = jmeTransform.getTranslation(); Quat4d quat = new Quat4d(jmeQuat.getX(), jmeQuat.getY(), jmeQuat.getZ(), jmeQuat.getW()); Vector3d vect = new Vector3d(jmeVect.getX(), jmeVect.getY(), jmeVect.getZ()); RigidBodyTransform ret = new RigidBodyTransform(quat, vect); return ret; } }
public static Transform3D transform(final PoseType.Pose position) { RotationType.Rotation pRotation = position.getRotation(); TranslationType.Translation pTranslation = position.getTranslation(); Quat4d jRotation = new Quat4d(pRotation.getQx(), pRotation.getQy(), pRotation.getQz(), pRotation.getQw()); Vector3d jTranslation = new Vector3d(pTranslation.getX(), pTranslation.getY(), pTranslation.getZ()); return new Transform3D(jRotation, jTranslation, 1.0); } }
public static us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage convertFootStepData(FootstepDataRosMessage msg) { RobotSide robotSide = RobotSide.values[(int) msg.getRobotSide()]; Vector3 loc = msg.getLocation(); Quaternion quat = msg.getOrientation(); Point3d location = new Point3d(loc.getX(), loc.getY(), loc.getZ()); Quat4d orientation = new Quat4d(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); return new us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage(robotSide, location, orientation); }