/** * Gets the read-only reference to the orientation of this shape. * * @return the orientation of this shape. */ public RotationMatrixReadOnly getOrientation() { return shapePose.getRotationMatrix(); }
/** * Computes and returns the roll angle from the yaw-pitch-roll representation of this shape * orientation. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @return the roll angle around the x-axis. */ public final double getOrientationRoll() { return shapePose.getRotationMatrix().getRoll(); }
/** * Computes and returns the yaw angle from the yaw-pitch-roll representation of this shape * orientation. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @return the yaw angle around the z-axis. */ public final double getOrientationYaw() { return shapePose.getRotationMatrix().getYaw(); }
/** * Computes and returns the pitch angle from the yaw-pitch-roll representation of this shape * orientation. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @return the pitch angle around the y-axis. */ public final double getOrientationPitch() { return shapePose.getRotationMatrix().getPitch(); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { position.set(transformToWorld.getTranslationVector()); if (isUsingYawPitchRoll()) yawPitchRoll.set(transformToWorld.getRotationMatrix()); else quaternion.set(transformToWorld.getRotationMatrix()); }
/** * Sets this pose 3D to match the given rigid-body transform. * * @param rigidBodyTransform the transform use to set this pose 3D. Not modified. */ default void set(RigidBodyTransform rigidBodyTransform) { setPosition(rigidBodyTransform.getTranslationVector()); setOrientation(rigidBodyTransform.getRotationMatrix()); }
public void set(RigidBodyTransform transform3D) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), transform3D.getRotationMatrix()); set(tempFrameOrientation); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { yoFramePoint.set(transformToWorld.getTranslationVector()); yoFrameYawPitchRoll.set(transformToWorld.getRotationMatrix()); }
/** * Sets the joint current configuration from the given transform. * <p> * The common implementation of this method is to project the transform onto the motion subspace of * this joint such not all the components of the transform are used to update this joint * configuration. For instance, a {@code SphericalJoint} only uses the orientation part and a * {@code RevoluteJoint} projects the orientation onto its joint axis to compute its new joint * angle. * </p> * <p> * When updating the configuration the joints of a robot, it is important to then call * {@link #updateFramesRecursively()} on the root joint to update all the reference frames to the * new robot configuration. * </p> * * @param jointConfiguration the transform containing the new configuration for this joint. Not * modified. */ default void setJointConfiguration(RigidBodyTransform jointConfiguration) { setJointOrientation(jointConfiguration.getRotationMatrix()); setJointPosition(jointConfiguration.getTranslationVector()); }
public static double getDistance(RigidBodyTransform from, RigidBodyTransform to, double positionWeight, double orientationWeight) { Point3D pointFrom = new Point3D(from.getTranslationVector()); Quaternion orientationFrom = new Quaternion(from.getRotationMatrix()); Point3D pointTo = new Point3D(to.getTranslationVector()); Quaternion orientationTo = new Quaternion(to.getRotationMatrix()); double positionDistance = positionWeight * pointFrom.distance(pointTo); double orientationDistance = orientationWeight * orientationFrom.distance(orientationTo); double distance = positionDistance + orientationDistance; return distance; }
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); }
public static VehiclePosePacket createVehiclePosePacket(RigidBodyTransform transformFromVehicleToWorld) { VehiclePosePacket message = new VehiclePosePacket(); message.getOrientation().set(transformFromVehicleToWorld.getRotationMatrix()); message.getPosition().set(transformFromVehicleToWorld.getTranslationVector()); return message; }
/** * Appends the given {@code transform} to this pose 3D. * * @param transform the rigid-body transform to append to this pose 3D. Not modified. */ default void appendTransform(RigidBodyTransform transform) { QuaternionTools.addTransform(getOrientation(), transform.getTranslationVector(), getPosition()); getOrientation().append(transform.getRotationMatrix()); }
/** * trajectory for current transform to goal transform */ public static Pose3D computeLinearTrajectory(double time, double trajectoryTime, RigidBodyTransform from, RigidBodyTransform to) { double progress = time / trajectoryTime; Point3D fromPoint = new Point3D(from.getTranslationVector()); Point3D toPoint = new Point3D(to.getTranslationVector()); Quaternion fromOrienation = new Quaternion(from.getRotationMatrix()); Quaternion toOrienation = new Quaternion(to.getRotationMatrix()); Point3D point = new Point3D(); Quaternion orientation = new Quaternion(); point.interpolate(fromPoint, toPoint, progress); orientation.interpolate(fromOrienation, toOrienation, progress); return new Pose3D(point, orientation); }
public void getAnkleOrientation(FrameQuaternion orientationToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); orientationToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getRotationMatrix()); }
/** * Sets this pose 2D to match the given rigid-body transform. * * @param rigidBodyTransform the transform use to set this pose 2D. Not modified. * @param checkIsTransform2D indicates whether or not the method should check that the rotation part * of the given transform represents a 2D rotation in the XY-plane. * @throws NotAMatrix2DException if {@code checkIsTransform2D} is {@code true} and if the rotation * part of the transform does not represent a 2D transformation. */ default void set(RigidBodyTransform rigidBodyTransform, boolean checkIsTransform2D) { if (checkIsTransform2D) rigidBodyTransform.checkIfRotation2D(); setPosition(rigidBodyTransform.getTranslationX(), rigidBodyTransform.getTranslationY()); setOrientation(rigidBodyTransform.getRotationMatrix().getYaw()); }
public static KinematicsPlanningToolboxRigidBodyMessage createKinematicsPlanningToolboxRigidBodyMessage(RigidBodyBasics endEffector, ReferenceFrame controlFrame, TDoubleArrayList keyFrameTimes, List<Pose3DReadOnly> keyFramePoses) { KinematicsPlanningToolboxRigidBodyMessage message = new KinematicsPlanningToolboxRigidBodyMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); RigidBodyTransform transformToBodyFixedFrame = new RigidBodyTransform(); controlFrame.getTransformToDesiredFrame(transformToBodyFixedFrame, endEffector.getBodyFixedFrame()); message.getControlFramePositionInEndEffector().set(transformToBodyFixedFrame.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(transformToBodyFixedFrame.getRotationMatrix()); if (keyFrameTimes.size() != keyFramePoses.size()) throw new RuntimeException("Inconsistent list lengths: keyFrameTimes.size() = " + keyFrameTimes.size() + ", keyFramePoses.size() = " + keyFramePoses.size()); for (int i = 0; i < keyFrameTimes.size(); i++) { message.getKeyFrameTimes().add(keyFrameTimes.get(i)); message.getKeyFramePoses().add().set(keyFramePoses.get(i)); } KinematicsPlanningToolboxMessageFactory.setDefaultAllowableDisplacement(message, keyFrameTimes.size()); return message; }
public boolean solveFor(FramePoint3DReadOnly position) { kinematicsToolboxController.requestInitialize(); FramePoint3D desiredPosition = new FramePoint3D(position); desiredPosition.changeFrame(ReferenceFrame.getWorldFrame()); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(endEffector, desiredPosition); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getControlFramePositionInEndEffector().set(controlFramePoseInEndEffector.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(controlFramePoseInEndEffector.getRotationMatrix()); commandInputManager.submitMessage(message); return solveAndRetry(50); }
/** * Transforms this spatial inertia by the inverse of the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> * * @param transform the transform to use on this. Not modified. */ default void applyInverseTransform(RigidBodyTransform transform) { if (transform.hasTranslation()) { // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(getMass(), getCenterOfMassOffset(), true, transform.getTranslationVector(), getMomentOfInertia()); getCenterOfMassOffset().sub(transform.getTranslationVector()); } if (transform.hasRotation()) { // Let's first apply the rotation onto the CoM and the mass moment of inertia: MecanoTools.inverseTransformSymmetricMatrix3D(transform.getRotationMatrix(), getMomentOfInertia()); getCenterOfMassOffset().applyInverseTransform(transform); } } }
public boolean solveFor(FramePoint3DReadOnly position, FrameQuaternionReadOnly orientation) { kinematicsToolboxController.requestInitialize(); FramePoint3D desiredPosition = new FramePoint3D(position); desiredPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameQuaternion desiredOrientation = new FrameQuaternion(orientation); desiredOrientation.changeFrame(ReferenceFrame.getWorldFrame()); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(endEffector, desiredPosition, desiredOrientation); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getControlFramePositionInEndEffector().set(controlFramePoseInEndEffector.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(controlFramePoseInEndEffector.getRotationMatrix()); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(angularSelection)); commandInputManager.submitMessage(message); return solveAndRetry(maximumNumberOfIterations.getIntegerValue()); }