/** * Extracts only the rotation part of a PoseType and converts it to a * PmRotationMatrix * * @param p Pose to convert * @return Rotation Vector from rotational component of pose. * @throws PmException if rotation vectors are invalid */ public static PmRotationVector toPmRotationVector(final PoseType p) throws PmException { return Posemath.toRot(toPmRotationMatrix(p)); }
PoseType pose = CRCLPosemath.toPoseType(cart, rcs.posemath.Posemath.toRot(rpy)); JbrPose jbrPose = new JbrPose("", index, pose); System.out.println("jbrPose = " + jbrPose);
/** * Compute the magnitude of a rotation vector between the two poses in * radians. * * @param pose1 first pose to compare * @param pose2 second pose to compare * @return magnitude of rotation between poses. * @throws PmException if either pose has an invalid rotation. */ public static double diffPosesRot(PoseType pose1, PoseType pose2) throws PmException { PmRotationMatrix m1 = toPmRotationMatrix(pose1); PmRotationMatrix m2 = toPmRotationMatrix(pose2); PmRotationVector r = Posemath.toRot(m1.multiply(m2.inv())); return r.s; }
PoseType pose = CRCLPosemath.toPoseType(posxyzwpr.cart, Posemath.toRot(posxyzwpr.rpy)); if (parts[2].endsWith("mm/sec")) { SetTransSpeedType setSpeedCmd = new SetTransSpeedType();
PmRotationVector rv = Posemath.toRot(rotMat);
PmCartesian cart = new PmCartesian(pos.x() / lengthScale, pos.y() / lengthScale, pos.z() / lengthScale); PmRpy rpy = new PmRpy(Math.toRadians(pos.w()), Math.toRadians(pos.p()), Math.toRadians(pos.r())); setPose(CRCLPosemath.toPoseType(cart, rcs.posemath.Posemath.toRot(rpy), getPose())); Com4jObject com4jobj_joint_pos = icgp.formats(FRETypeCodeConstants.frJoint); IJoint joint_pos = com4jobj_joint_pos.queryInterface(IJoint.class);