PmRotationMatrix mat = Posemath.toMat(zyx);//new PmRotationVector(rotMag, rx / rotMag, ry / rotMag, rz / rotMag); crclStatus.getPoseStatus().setPose(CRCLPosemath.toPoseType(cart, mat, crclStatus.getPoseStatus().getPose()));
/** * Combine a translation and rotation in a PoseType * * @param tran translational component of pose * @param v rotational component of pose * @param pose_in optional pose to be set instead of creating new Pose * @return new Pose creating from combining inputs or pose_in if not null * @throws PmException if rotation vector can not be converted to matrix */ static public PoseType toPoseType(PmCartesian tran, PmRotationVector v, /*@Nullable*/ PoseType pose_in) throws PmException { PoseType pose = pose_in; if (pose == null) { pose = new PoseType(); } pose.setPoint(toPointType(tran)); PmRotationMatrix mat = Posemath.toMat(v); VectorType xVec = new VectorType(); xVec.setI(mat.x.x); xVec.setJ(mat.x.y); xVec.setK(mat.x.z); pose.setXAxis(xVec); VectorType zVec = new VectorType(); zVec.setI(mat.z.x); zVec.setJ(mat.z.y); zVec.setK(mat.z.z); pose.setZAxis(zVec); return pose; }
/** * Combine an rcslib Posemath translation and rotation(roll-pitch-yaw) in a * PoseType * * @param tran translational component of pose * @param v rotational component of pose in roll-pith-yaw format. * @param pose_in optional pose to be set instead of creating new Pose * @return new Pose creating from combining inputs or pose_in if not null * @throws PmException if rotation vector can not be converted to matrix */ static public PoseType toPoseType(PmCartesian tran, PmRpy v, /*@Nullable*/ PoseType pose_in) throws PmException { PoseType pose = pose_in; if (pose == null) { pose = new PoseType(); } pose.setPoint(toPointType(tran)); PmRotationMatrix mat = Posemath.toMat(v); VectorType xVec = new VectorType(); xVec.setI(mat.x.x); xVec.setJ(mat.x.y); xVec.setK(mat.x.z); pose.setXAxis(xVec); VectorType zVec = new VectorType(); zVec.setI(mat.z.x); zVec.setJ(mat.z.y); zVec.setK(mat.z.z); pose.setZAxis(zVec); return pose; }