public Quaternion getOrientationCopy() { Quaternion orientationCopy = new Quaternion(); getOrientation(orientationCopy); return orientationCopy; }
public SO3TrajectoryPointMessage() { orientation_ = new us.ihmc.euclid.tuple4D.Quaternion(); angular_velocity_ = new us.ihmc.euclid.tuple3D.Vector3D(); }
public static Quaternion nextQuaternion(Random random, double minMaxAngleRange) { AxisAngle orientation = nextAxisAngle(random, minMaxAngleRange); Quaternion quat = new Quaternion(); quat.set(orientation); return quat; }
public KinematicsToolboxOutputStatus() { desired_joint_angles_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5"); desired_root_translation_ = new us.ihmc.euclid.tuple3D.Vector3D(); desired_root_orientation_ = new us.ihmc.euclid.tuple4D.Quaternion(); }
public InteractiveMarkerControl() { name_ = new java.lang.StringBuilder(255); orientation_ = new us.ihmc.euclid.tuple4D.Quaternion(); markers_ = new us.ihmc.idl.IDLSequence.Object<visualization_msgs.msg.dds.Marker> (100, new visualization_msgs.msg.dds.MarkerPubSubType()); description_ = new java.lang.StringBuilder(255); }
private PelvisOrientationTrajectoryMessage createPelvisOrientationTrajectoryMessage(Vector3D rotationAxis, double rotationAngle) { AxisAngle desiredPelvisAxisAngle = new AxisAngle(rotationAxis, rotationAngle); Quaternion desiredPelvisQuat = new Quaternion(); desiredPelvisQuat.set(desiredPelvisAxisAngle); PelvisOrientationTrajectoryMessage pelvisOrientationTrajectoryMessage = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(3.0, desiredPelvisQuat); return pelvisOrientationTrajectoryMessage; }
public TestingEnvironment() { terrain = new CombinedTerrainObject3D(getClass().getSimpleName()); terrain.addBox(-0.2, -0.225, 3.2, 0.225, -0.1, 0.0); terrain.addBox(0.15, 0.05, 0.45, 0.25, 0.15); terrain.addBox(0.6, -0.05, 0.775, -0.25, 0.08); terrain.addCylinder(new RigidBodyTransform(new Quaternion(0.0, 0.0, 0.0, 1.0), new Point3D(1.5, 0.15, 0.1)), 0.2, 0.15, YoAppearance.Grey()); terrain.addCylinder(new RigidBodyTransform(new Quaternion(0.0, 0.0, 0.0, 1.0), new Point3D(1.8, -0.15, 0.1)), 0.2, 0.025, YoAppearance.Grey()); terrain.addBox(1.96, 0.125, 1.99, 0.0, 0.2); terrain.addBox(2.235, 0.175, 2.265, 0.25, 0.2); }
private static double getAngleDifference(Quaternion q1, Quaternion q2) { Quaternion qDifference = new Quaternion(q1); qDifference.multiplyConjugateOther(q2); AxisAngle axisAngle = new AxisAngle(); axisAngle.set(qDifference); return axisAngle.getAngle(); } }
@Override protected void doRotateInstruction(Graphics3DRotateInstruction graphics3dObjectRotateMatrix) { QuaternionBasics quat4d = new us.ihmc.euclid.tuple4D.Quaternion(graphics3dObjectRotateMatrix.getRotationMatrix()); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // quat4d.set(graphics3dObjectRotateMatrix.getRotationMatrix()); Quaternion quaternion = new Quaternion(); JMEDataTypeUtils.packVectMathQuat4dInJMEQuaternion(quat4d, quaternion); rotate(quaternion); }
private boolean isOrientationEqual(QuaternionReadOnly initialQuat, QuaternionReadOnly finalQuat, double angleEpsilon) { Quaternion quatDifference = new Quaternion(initialQuat); quatDifference.multiplyConjugateOther(finalQuat); AxisAngle angleDifference = new AxisAngle(); angleDifference.set(quatDifference); AngleTools.trimAngleMinusPiToPi(angleDifference.getAngle()); return Math.abs(angleDifference.getAngle()) < angleEpsilon; }
public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, RotationMatrixReadOnly rotationMatrix) { WallPosePacket message = new WallPosePacket(); message.setCuttingRadius(cuttingRadius); message.getCenterPosition().set(centerPosition); message.getCenterOrientation().set(new Quaternion(rotationMatrix)); return message; }
private void submitFootPose(RobotSide robotSide, FramePose3D desiredFootPose) { desiredFootPose.changeFrame(worldFrame); Point3D desiredFootPosition = new Point3D(); Quaternion desiredFootOrientation = new Quaternion(); desiredFootPose.get(desiredFootPosition, desiredFootOrientation); FootTrajectoryTask task = new FootTrajectoryTask(robotSide, desiredFootPosition, desiredFootOrientation, footTrajectoryBehavior, trajectoryTime.getDoubleValue()); pipeLine.submitSingleTaskStage(task); }
private Footstep generateFootstep(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { double yaw = footPose2d.getYaw(); Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation); Footstep footstep = new Footstep(robotSide, footstepPose); return footstep; }
public QuaternionBasics getCameraRotation() { Quaternion quat = new Quaternion(getRotation()); JMEGeometryUtils.transformFromJMECoordinatesToZup(quat); quat.multLocal(convertLookingForwardToLookingUp); return new us.ihmc.euclid.tuple4D.Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); }
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quaternion desiredQuaternion = new Quaternion(); FrameQuaternion desiredOrientation = new FrameQuaternion(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredQuaternion.set(desiredOrientation); ReferenceFrame pelvisZUpFrame = referenceFrames.getPelvisZUpFrame(); ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredQuaternion, worldFrame, pelvisZUpFrame); output.getChestTrajectoryMessage().set(chestTrajectoryMessage); }
public void computePelvisTrajectoryMessage() { checkIfDataHasBeenSet(); Point3D desiredPosition = new Point3D(); Quaternion desiredOrientation = new Quaternion(); ReferenceFrame pelvisFrame = fullRobotModelToUseForConversion.getRootJoint().getFrameAfterJoint(); FramePose3D desiredPelvisPose = new FramePose3D(pelvisFrame); desiredPelvisPose.changeFrame(worldFrame); desiredPelvisPose.get(desiredPosition, desiredOrientation); PelvisTrajectoryMessage pelvisTrajectoryMessage = HumanoidMessageTools.createPelvisTrajectoryMessage(trajectoryTime, desiredPosition, desiredOrientation); output.getPelvisTrajectoryMessage().set(pelvisTrajectoryMessage); }
private ChestTrajectoryMessage createRandomChestMessage(double trajectoryTime, Random random) { OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); Quaternion desiredOrientation = new Quaternion(desiredRandomChestOrientation); return HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, ReferenceFrame.getWorldFrame(), ReferenceFrame.getWorldFrame()); }
public static RigidBodyTransform jmeTransformToTransform3D(Transform jmeTransform) { Quaternion jmeQuat = jmeTransform.getRotation(); Vector3f jmeVect = jmeTransform.getTranslation(); us.ihmc.euclid.tuple4D.Quaternion quat = new us.ihmc.euclid.tuple4D.Quaternion(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 Quaternion findQuat4d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { double x = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(prefix, suffix)).getValueAsDouble(); double y = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(prefix, suffix)).getValueAsDouble(); double z = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(prefix, suffix)).getValueAsDouble(); double s = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(prefix, suffix)).getValueAsDouble(); return new Quaternion(x, y, z, s); }
private void executeMessage(ChestTrajectoryMessage message, RigidBodyBasics chest) throws SimulationExceededMaximumTimeException { double controllerDT = getRobotModel().getControllerDT(); drcSimulationTestHelper.publishToController(message); double trajectoryTime = message.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime(); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 5.0 * controllerDT)); Quaternion desired = new Quaternion(message.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getOrientation()); assertChestDesired(drcSimulationTestHelper.getSimulationConstructionSet(), desired, chest); }