public void setRotation(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, jointRotation); this.afterJointFrame.setRotation(jointRotation); }
public void setRotation(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToQuaternion(0.0, pitch, 0.0, jointRotation); afterJointFrame.setRotation(this.jointRotation); }
public void getRotation(Quat4d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { if (Double.isNaN(roll)) { throw new RuntimeException("Orientation.setYawPitchRoll(). yaw = " + yaw + ", pitch = " + pitch + ", roll = " + roll); } RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, quaternion); normalize(); }
public void setRotation(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, jointRotation); this.afterJointFrame.setRotation(jointRotation); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, this); if (!containsNaN()) normalize(); }
public void setRotation(Quat4d jointRotation) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void getRotation(Quat4f rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); rotationToPack.set(jointRotation); }
public void setYawPitchRoll(double[] yawPitchRoll) { RotationTools.convertYawPitchRollToQuaternion(yawPitchRoll, this); if (!containsNaN()) normalize(); }
public void setRotation(double x, double y, double z, double w) { RotationTools.convertQuaternionToYawPitchRoll(x, y, z, w, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
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 void setRotation(Matrix3d jointRotation) { RotationTools.convertMatrixToYawPitchRoll(jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void getQuaternion(Quat4d quaternionToPack) { RotationTools.convertYawPitchRollToQuaternion(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue(), quaternionToPack); }
private void createValve(String valveRobotName, ValveType valveType, double x, double y, double z, double yaw_degrees, double forceVectorScale) { FramePose valvePose = new FramePose(ReferenceFrame.getWorldFrame()); Point3d position = new Point3d(x, y, z); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0), orientation); valvePose.setPose(position, orientation); ContactableValveRobot valve = new ContactableValveRobot(valveRobotName, valveType, 0.5, valvePose); valve.createValveRobot(); valve.createAvailableContactPoints(1, 30, forceVectorScale, true); valveRobot.add(valve); }
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; }
public DRCDebrisEnvironment(Tuple3d robotInitialPosition, double robotInitialYaw) { combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(setUpGround("Ground")); Quat4d robotInitialOrientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(robotInitialYaw, 0.0, 0.0, robotInitialOrientation); Point3d robotPosition = new Point3d(robotInitialPosition); FramePose robotInitialPose = new FramePose(constructionWorldFrame, robotPosition, robotInitialOrientation); this.robotInitialPoseReferenceFrame = new PoseReferenceFrame("robotInitialPoseReferenceFrame", robotInitialPose); }
public void initialize(DenseMatrix64F accel, DenseMatrix64F pqr, double heading) { setMatrix(bias, pqr); // euler = accel2euler(accel, heading); accel2euler(accel, heading); Quat4d quaternion = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(eulerAngles, quaternion); MatrixTools.insertQuat4dIntoEJMLVector(q, quaternion, 0); }
public void setPositionAndRotation(RigidBodyTransform transform) { RotationTools.convertTransformToYawPitchRoll(transform, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, jointRotation); if (!RotationTools.isQuaternionNormalized(jointRotation)) { throw new AssertionError("quaternion is not normalized. " + jointRotation); } transform.getTranslation(jointTranslation); jointTranslation.setY(0.0); this.afterJointFrame.setRotation(jointRotation); this.afterJointFrame.setTranslation(jointTranslation); }
@Override public void setConfiguration(DenseMatrix64F matrix, int rowStart) { int index = rowStart; double qRot = matrix.get(index++, 0); double x = matrix.get(index++, 0); double z = matrix.get(index++, 0); RotationTools.convertYawPitchRollToQuaternion(0.0, qRot, 0.0, jointRotation); jointTranslation.set(x, 0.0, z); afterJointFrame.setRotation(jointRotation); afterJointFrame.setTranslation(jointTranslation); }
private void setYawPitchRoll() { Quat4d q = new Quat4d(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). RotationTools.convertYawPitchRollToQuaternion(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue(), q); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.mul(qprev); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }