diff --git a/docs/source/reference/index.rst b/docs/source/reference/index.rst index aea19bf..77e636a 100644 --- a/docs/source/reference/index.rst +++ b/docs/source/reference/index.rst @@ -8,6 +8,7 @@ API Reference PlanningWorld core/ArticulatedModel core/AttachedBody + utils/pose utils/random .. toctree:: diff --git a/docs/source/reference/utils/pose.rst b/docs/source/reference/utils/pose.rst new file mode 100644 index 0000000..81fda08 --- /dev/null +++ b/docs/source/reference/utils/pose.rst @@ -0,0 +1,7 @@ +``Pose`` +------------------------- + +.. autoclass:: mplib.pymp.Pose + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/tutorials/plan_a_path.rst b/docs/source/tutorials/plan_a_path.rst index 835789e..4568576 100644 --- a/docs/source/tutorials/plan_a_path.rst +++ b/docs/source/tutorials/plan_a_path.rst @@ -92,7 +92,7 @@ In this example, we create some boxes inside the simulation like so: :start-after: # boxes ankor :end-before: # boxes ankor end -We then find the target poses needed to reach the boxes. The poses are specified in [x,y,z,qw,qx,qy,qz]: +We then find the target poses needed to reach the boxes. .. literalinclude:: ../../../mplib/examples/demo.py :dedent: 0 diff --git a/include/mplib/collision_detection/fcl/collision_common.h b/include/mplib/collision_detection/fcl/collision_common.h index ea2cca0..37b635c 100644 --- a/include/mplib/collision_detection/fcl/collision_common.h +++ b/include/mplib/collision_detection/fcl/collision_common.h @@ -9,6 +9,7 @@ #include "mplib/collision_detection/fcl/types.h" #include "mplib/macros/class_forward.h" #include "mplib/types.h" +#include "mplib/utils/pose.h" namespace mplib::collision_detection::fcl { @@ -42,9 +43,9 @@ struct FCLObject { * @param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr`` * @param shape_poses: relative poses from this FCLObject to each collision shape */ - FCLObject(const std::string &name, const Isometry3 &pose, - const std::vector> &shapes, - const std::vector> &shape_poses); + FCLObject(const std::string &name_, const Pose &pose_, + const std::vector> &shapes_, + const std::vector> &shape_poses_); std::string name; ///< Name of this FCLObject Isometry3 pose; ///< Pose of this FCLObject. All shapes are relative to this pose diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 0fd8813..7175916 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -12,7 +12,7 @@ #include "mplib/collision_detection/fcl/collision_common.h" #include "mplib/macros/class_forward.h" -#include "mplib/types.h" +#include "mplib/utils/pose.h" namespace mplib::collision_detection::fcl { @@ -125,14 +125,7 @@ class FCLModelTpl { * * @param link_poses: list of link poses in the order of the link order */ - void updateCollisionObjects(const std::vector> &link_poses) const; - - /** - * Update the collision objects of the FCL model. - * - * @param link_poses: list of link poses in the order of the link order - */ - void updateCollisionObjects(const std::vector> &link_poses) const; + void updateCollisionObjects(const std::vector> &link_poses) const; /** * Perform self-collision checking. diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index 0110502..a689d8d 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -9,7 +9,7 @@ #include "mplib/kinematics/types.h" #include "mplib/macros/class_forward.h" #include "mplib/types.h" -#include "mplib/utils/conversion.h" +#include "mplib/utils/pose.h" namespace mplib { @@ -66,6 +66,7 @@ class ArticulatedModelTpl { * @param verbose: print debug information. Default: ``false``. * @return: a unique_ptr to ArticulatedModel */ + // TODO(merge): remove pair (link name is not needed) static std::unique_ptr> createFromURDFString( const std::string &urdf_string, const std::string &srdf_string, const std::vector>> @@ -176,7 +177,7 @@ class ArticulatedModelTpl { const VectorX &getQpos() const { return current_qpos_; } /** - * Let the planner know the current joint positions. + * Set the current joint positions and update all collision links in the ``FCLModel``. * * @param qpos: current qpos of all active joints or just the move group joints * @param full: whether to set the full qpos or just the move group qpos. @@ -186,18 +187,21 @@ class ArticulatedModelTpl { void setQpos(const VectorX &qpos, bool full = false); /** - * Get the base pose of the robot. + * Get the base (root) pose of the robot. * - * @return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format + * @return: base pose of the robot */ - Vector7 getBasePose() const { return toPoseVec(base_pose_); } + Pose getBasePose() const { return Pose(base_pose_); } /** - * Set the base pose of the robot. + * Set the base pose of the robot and update all collision links in the ``FCLModel``. * - * @param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format + * @param pose: base pose of the robot */ - void setBasePose(const Vector7 &pose); + void setBasePose(const Pose &pose) { + base_pose_ = pose.toIsometry(); + setQpos(current_qpos_, true); // update all collision links in the fcl_model_ + } /** * Update the SRDF file to disable self-collisions. diff --git a/include/mplib/core/attached_body.h b/include/mplib/core/attached_body.h index 651329a..4a71f50 100644 --- a/include/mplib/core/attached_body.h +++ b/include/mplib/core/attached_body.h @@ -8,7 +8,7 @@ #include "mplib/kinematics/types.h" #include "mplib/macros/class_forward.h" #include "mplib/types.h" -#include "mplib/utils/conversion.h" +#include "mplib/utils/pose.h" namespace mplib { @@ -42,7 +42,7 @@ class AttachedBodyTpl { */ AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object, const ArticulatedModelPtr &attached_articulation, - int attached_link_id, const Isometry3 &pose, + int attached_link_id, const Pose &pose, const std::vector &touch_links = {}); /// @brief Gets the attached object name @@ -51,10 +51,10 @@ class AttachedBodyTpl { /// @brief Gets the attached object (``FCLObjectPtr``) FCLObjectPtr getObject() const { return object_; } - /// @brief Gets the articulation this body is attached to + /// @brief Gets the articulation that this body is attached to ArticulatedModelPtr getAttachedArticulation() const { return attached_articulation_; } - /// @brief Gets the articulation this body is attached to + /// @brief Gets the articulation link id that this body is attached to int getAttachedLinkId() const { return attached_link_id_; } /// @brief Gets the attached pose (relative pose from attached link to object) @@ -63,11 +63,14 @@ class AttachedBodyTpl { /// @brief Sets the attached pose (relative pose from attached link to object) void setPose(const Isometry3 &pose) { pose_ = pose; } - /// @brief Gets the global pose of the attached object - Isometry3 getGlobalPose() const { - return toIsometry(pinocchio_model_->getLinkPose(attached_link_id_)) * pose_; + /// @brief Gets the global pose of the articulation link that this body is attached to + Isometry3 getAttachedLinkGlobalPose() const { + return pinocchio_model_->getLinkPose(attached_link_id_); } + /// @brief Gets the global pose of the attached object + Isometry3 getGlobalPose() const { return getAttachedLinkGlobalPose() * pose_; } + /// @brief Updates the global pose of the attached object using current state void updatePose() const; diff --git a/include/mplib/kinematics/kdl/kdl_model.h b/include/mplib/kinematics/kdl/kdl_model.h index 2e6c88d..256d183 100644 --- a/include/mplib/kinematics/kdl/kdl_model.h +++ b/include/mplib/kinematics/kdl/kdl_model.h @@ -9,6 +9,7 @@ #include "mplib/macros/class_forward.h" #include "mplib/types.h" +#include "mplib/utils/pose.h" namespace mplib::kinematics::kdl { @@ -30,19 +31,18 @@ class KDLModelTpl { const std::string &getTreeRootName() const { return tree_root_name_; } std::tuple, int> chainIKLMA(size_t index, const VectorX &q0, - const Vector7 &pose) const; + const Pose &pose) const; std::tuple, int> chainIKNR(size_t index, const VectorX &q0, - const Vector7 &pose) const; + const Pose &pose) const; std::tuple, int> chainIKNRJL(size_t index, const VectorX &q0, - const Vector7 &pose, - const VectorX &q_min, + const Pose &pose, const VectorX &q_min, const VectorX &q_max) const; std::tuple, int> TreeIKNRJL(const std::vector endpoints, const VectorX &q0, - const std::vector> &poses, + const std::vector> &poses, const VectorX &q_min, const VectorX &q_max) const; diff --git a/include/mplib/kinematics/pinocchio/pinocchio_model.h b/include/mplib/kinematics/pinocchio/pinocchio_model.h index 31a67db..50d4343 100644 --- a/include/mplib/kinematics/pinocchio/pinocchio_model.h +++ b/include/mplib/kinematics/pinocchio/pinocchio_model.h @@ -14,6 +14,7 @@ #include "mplib/kinematics/pinocchio/types.h" #include "mplib/macros/class_forward.h" #include "mplib/types.h" +#include "mplib/utils/pose.h" namespace mplib::kinematics::pinocchio { @@ -266,15 +267,15 @@ class PinocchioModelTpl { void computeForwardKinematics(const VectorX &qpos); /** - * Get the pose of the given link. + * Get the pose of the given link in robot's base (root) frame. * * @param index: index of the link (in the order you passed to the constructor or the * default order) - * @return: pose of the link [x, y, z, qw, qx, qy, qz] + * @return: pose of the link in robot's base (root) frame. */ - Vector7 getLinkPose(size_t index) const; + Isometry3 getLinkPose(size_t index) const; - Vector7 getJointPose(size_t index) const; // TODO: not same as sapien + Isometry3 getJointPose(size_t index) const; // TODO: not same as sapien /** * Compute the full jacobian for the given joint configuration. @@ -318,7 +319,7 @@ class PinocchioModelTpl { * * @param index: index of the link (in the order you passed to the constructor or the * default order) - * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] + * @param pose: desired pose of the link * @param q_init: initial joint configuration * @param mask: if the value at a given index is ``true``, the joint is *not* used in * the IK @@ -329,7 +330,7 @@ class PinocchioModelTpl { * @return: joint configuration */ std::tuple, bool, Vector6> computeIKCLIK( - size_t index, const Vector7 &pose, const VectorX &q_init, + size_t index, const Pose &pose, const VectorX &q_init, const std::vector &mask, double eps = 1e-5, int max_iter = 1000, double dt = 1e-1, double damp = 1e-12); @@ -339,7 +340,7 @@ class PinocchioModelTpl { * * @param index: index of the link (in the order you passed to the constructor or the * default order) - * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] + * @param pose: desired pose of the link * @param q_init: initial joint configuration * @param q_min: minimum joint configuration * @param q_max: maximum joint configuration @@ -350,7 +351,7 @@ class PinocchioModelTpl { * @return: joint configuration */ std::tuple, bool, Vector6> computeIKCLIKJL( - size_t index, const Vector7 &pose, const VectorX &q_init, + size_t index, const Pose &pose, const VectorX &q_init, const VectorX &qmin, const VectorX &qmax, double eps = 1e-5, int max_iter = 1000, double dt = 1e-1, double damp = 1e-12); diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 6aa412a..66bf05f 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -10,6 +10,7 @@ #include "mplib/core/attached_body.h" #include "mplib/macros/class_forward.h" #include "mplib/types.h" +#include "mplib/utils/pose.h" namespace mplib { @@ -265,8 +266,7 @@ class PlanningWorldTpl { * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id, - const Vector7 &pose, - const std::vector &touch_links); + const Pose &pose, const std::vector &touch_links); /** * Attaches existing normal object to specified link of articulation at given pose. @@ -284,7 +284,7 @@ class PlanningWorldTpl { * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id, - const Vector7 &pose); + const Pose &pose); /** * Attaches given object (w/ p_geom) to specified link of articulation at given pose. @@ -299,7 +299,7 @@ class PlanningWorldTpl { * @param touch_links: link names that the attached object touches */ void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, - const std::string &art_name, int link_id, const Vector7 &pose, + const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links); /** @@ -316,7 +316,7 @@ class PlanningWorldTpl { * @param pose: attached pose (relative pose from attached link to object) */ void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, - const std::string &art_name, int link_id, const Vector7 &pose); + const std::string &art_name, int link_id, const Pose &pose); /** * Attaches given sphere to specified link of articulation (auto touch_links) @@ -327,7 +327,7 @@ class PlanningWorldTpl { * @param pose: attached pose (relative pose from attached link to object) */ void attachSphere(S radius, const std::string &art_name, int link_id, - const Vector7 &pose); + const Pose &pose); /** * Attaches given box to specified link of articulation (auto touch_links) @@ -338,7 +338,7 @@ class PlanningWorldTpl { * @param pose: attached pose (relative pose from attached link to object) */ void attachBox(const Vector3 &size, const std::string &art_name, int link_id, - const Vector7 &pose); + const Pose &pose); /** * Attaches given mesh to specified link of articulation (auto touch_links) @@ -349,7 +349,7 @@ class PlanningWorldTpl { * @param pose: attached pose (relative pose from attached link to object) */ void attachMesh(const std::string &mesh_path, const std::string &art_name, - int link_id, const Vector7 &pose); + int link_id, const Pose &pose); /** * Detaches object with given name. diff --git a/include/mplib/types.h b/include/mplib/types.h index 754d911..f97ff46 100644 --- a/include/mplib/types.h +++ b/include/mplib/types.h @@ -14,9 +14,6 @@ using Vector4 = Eigen::Matrix; template using Vector6 = Eigen::Matrix; -template -using Vector7 = Eigen::Matrix; - template using VectorX = Eigen::Matrix; diff --git a/include/mplib/utils/conversion.h b/include/mplib/utils/conversion.h index cb56c7e..b0bdc66 100644 --- a/include/mplib/utils/conversion.h +++ b/include/mplib/utils/conversion.h @@ -9,14 +9,6 @@ namespace mplib { -/// Converts an Eigen::Isometry3 matrix to a pose_vec [px, py, pz, qw, qx, qy, qz] -template -Vector7 toPoseVec(const Isometry3 &pose); - -/// Converts a pose_vec [px, py, pz, qw, qx, qy, qz] to an Eigen::Isometry3 matrix -template -Isometry3 toIsometry(const Vector7 &pose_vec); - template Isometry3 toIsometry(const pinocchio::SE3Tpl &T); @@ -37,8 +29,6 @@ pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y); // Explicit Template Instantiation Declaration ========================================= #define DECLARE_TEMPLATE_CONVERSION(S) \ - extern template Vector7 toPoseVec(const Isometry3 &pose); \ - extern template Isometry3 toIsometry(const Vector7 &pose_vec); \ extern template Isometry3 toIsometry(const pinocchio::SE3Tpl &T); \ extern template Isometry3 toIsometry(const urdf::Pose &M); \ extern template pinocchio::SE3Tpl toSE3(const Isometry3 &T); \ diff --git a/include/mplib/utils/pose.h b/include/mplib/utils/pose.h new file mode 100644 index 0000000..20cb386 --- /dev/null +++ b/include/mplib/utils/pose.h @@ -0,0 +1,105 @@ +#pragma once + +#include "mplib/types.h" + +namespace mplib { + +/** + * Pose stored as a unit quaternion and a position vector + * + * This struct is intended to be used only for interfacing with Python. + * Internally, ``Pose`` is converted to and stored as ``Eigen::Isometry3`` + * which is used by all computations. + */ +template +struct Pose { + /// @brief Constructs a default Pose with p = (0,0,0) and q = (1,0,0,0) + Pose() : p(0.0, 0.0, 0.0), q(1.0, 0.0, 0.0, 0.0) {} + + /** + * Constructs a Pose with given position and quaternion + * + * @param p: position, format: (x, y, z) + * @param q: quaternion (can be unnormalized), format: (w, x, y, z) + */ + Pose(const Vector3 &p, const Vector4 &q) + : p(p), q(Quaternion {q(0), q(1), q(2), q(3)}.normalized()) {} + + /** + * Constructs a Pose with given position and quaternion + * + * @param p: position, format: (x, y, z) + * @param q: quaternion (can be unnormalized), format: (w, x, y, z) + */ + Pose(const Vector3 &p, const Quaternion &q) : p(p), q(q.normalized()) {} + + /** + * Constructs a Pose from a given ``Eigen::Isometry3`` instance + * + * @param pose: an ``Eigen::Isometry3`` instance + */ + Pose(const Isometry3 &pose) : p(pose.translation()), q(pose.linear()) {} + + /** + * Converts the Pose to an ``Eigen::Isometry3`` instance + * + * @return: an ``Eigen::Isometry3`` instance + */ + Isometry3 toIsometry() const { + Isometry3 ret; + ret.linear() = q.toRotationMatrix(); + ret.translation() = p; + return ret; + } + + /** + * Get the inserse Pose + * + * @return: the inverse Pose + */ + Pose inverse() const { + Quaternion qinv = q.conjugate(); // can use conjugate() since always normalized + return {qinv * -p, qinv}; + } + + /** + * Computes the distance between two poses by + * ``norm(p1.p - p2.p) + min(norm(p1.q - p2.q), norm(p1.q + p2.q))`. + * + * The quaternion part has range [0, sqrt(2)]. + * + * @param other: the other pose + * @return: the distance between the two poses + */ + S distance(const Pose &other) const { + return (p - other.p).norm() + std::min((q.coeffs() - other.q.coeffs()).norm(), + (q.coeffs() + other.q.coeffs()).norm()); + } + + /// @brief Overloading operator * for ``Pose * Vector3`` + Vector3 operator*(const Vector3 &v) const { return q * v + p; }; + + /// @brief Overloading operator * for ``Pose * Pose`` + Pose operator*(const Pose &other) const { + return {q * other.p + p, q * other.q}; + }; + + /// @brief Overloading operator *= for ``Pose *= Pose`` + Pose &operator*=(const Pose &other) { + *this = *this * other; + return *this; + }; + + /// @brief Overloading operator << + friend std::ostream &operator<<(std::ostream &out, const Pose &pose) { + out << "Pose([" << pose.p(0) << ", " << pose.p(1) << ", " << pose.p(2) << "], [" + << pose.q.w() << ", " << pose.q.x() << ", " << pose.q.y() << ", " << pose.q.z() + << "])"; + return out; + } + + Vector3 p {0.0, 0.0, 0.0}; ///< Position part of the Pose (x, y, z) + Quaternion q {1.0, 0.0, 0.0, 0.0}; ///< Quaternion part of the Pose (w, x, y, z) +}; + +} // namespace mplib diff --git a/mplib/__init__.py b/mplib/__init__.py index 5a6ce88..c114b83 100644 --- a/mplib/__init__.py +++ b/mplib/__init__.py @@ -4,6 +4,7 @@ from mplib.pymp import ( ArticulatedModel, PlanningWorld, + Pose, collision_detection, kinematics, planning, diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py index f42e4d5..b52427b 100644 --- a/mplib/examples/demo_setup.py +++ b/mplib/examples/demo_setup.py @@ -4,6 +4,7 @@ from sapien.utils.viewer import Viewer import mplib +from mplib import Pose class DemoSetup: @@ -160,12 +161,12 @@ def open_gripper(self): def close_gripper(self): self.set_gripper(0) - def move_to_pose_with_RRTConnect(self, pose): + def move_to_pose_with_RRTConnect(self, pose: Pose): """ Plan and follow a path to a pose using RRTConnect Args: - pose: [x, y, z, qx, qy, qz, qw] + pose: mplib.Pose """ # result is a dictionary with keys 'status', 'time', 'position', 'velocity', # 'acceleration', 'duration' diff --git a/mplib/planner.py b/mplib/planner.py index 3abef40..bd4978c 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -3,17 +3,17 @@ import os import xml.etree.ElementTree as ET from collections.abc import Callable -from typing import Optional, Sequence +from typing import Literal, Optional, Sequence from xml.dom import minidom import numpy as np import toppra as ta import toppra.algorithm as algo import toppra.constraint as constraint -from transforms3d.quaternions import mat2quat, quat2mat -from mplib.pymp import ArticulatedModel, PlanningWorld +from mplib.pymp import ArticulatedModel, PlanningWorld, Pose from mplib.pymp.collision_detection import AllowedCollisionMatrix, WorldCollisionResult +from mplib.pymp.collision_detection.fcl import CollisionGeometry from mplib.pymp.planning import ompl @@ -249,20 +249,6 @@ def generate_collision_pair(self, num_samples=100000): ) print(f"Saving the SRDF file to {self.srdf}") - def distance_6D(self, p1, q1, p2, q2): - """ - compute the distance between two poses - - Args: - p1: position of pose 1 - q1: quaternion of pose 1 - p2: position of pose 2 - q2: quaternion of pose 2 - """ - return np.linalg.norm(p1 - p2) + min( - np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2) - ) - def wrap_joint_limit(self, qpos: np.ndarray) -> bool: """ Checks if the joint configuration can be wrapped to be within the joint limits. @@ -367,7 +353,7 @@ def check_for_env_collision( def IK( self, - goal_pose: np.ndarray, + goal_pose: Pose, start_qpos: np.ndarray, mask: Optional[Sequence[bool] | np.ndarray] = None, *, @@ -379,12 +365,12 @@ def IK( """ Compute inverse kinematics - :param goal_pose: goal pose (xyz, wxyz), (7,) np.floating np.ndarray. + :param goal_pose: goal pose :param start_qpos: initial configuration, (ndof,) np.floating np.ndarray. :param mask: qpos mask to disable IK sampling, (ndof,) bool np.ndarray. :param n_init_qpos: number of random initial configurations to sample. - :param threshold: distance_6D threshold for marking sampled IK as success. - distance_6D is position error norm + quaternion error norm. + :param threshold: distance threshold for marking sampled IK as success. + distance is position error norm + quaternion error norm. :param return_closest: whether to return the qpos that is closest to start_qpos, considering equivalent joint values. :param verbose: whether to print collision info if any collision exists. @@ -402,7 +388,7 @@ def IK( move_joint_idx = self.move_group_joint_indices self.robot.set_qpos(start_qpos, True) - min_dis = 1e9 + min_dist = 1e9 q_goals = [] qpos = start_qpos for _ in range(n_init_qpos): @@ -430,12 +416,9 @@ def IK( if success: self.pinocchio_model.compute_forward_kinematics(ik_qpos) new_pose = self.pinocchio_model.get_link_pose(move_link_idx) - tmp_dis = self.distance_6D( - goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:] - ) - if tmp_dis < min_dis: - min_dis = tmp_dis - if tmp_dis < threshold: + if (dist := goal_pose.distance(new_pose)) < min_dist: + min_dist = dist + if dist < threshold: for q_goal in q_goals: if ( np.linalg.norm( @@ -452,8 +435,8 @@ def IK( if len(q_goals) != 0: status = "Success" - elif min_dis != 1e9: - status = f"IK Failed! Distance {min_dis} is greater than {threshold=}." + elif min_dist != 1e9: + status = f"IK Failed! Distance {min_dist} is greater than {threshold=}." return status, None else: status = "IK Failed! Cannot find valid solution." @@ -532,8 +515,8 @@ def remove_point_cloud(self, name="scene_pcd") -> bool: def update_attached_object( self, - collision_geometry, - pose, + collision_geometry: CollisionGeometry, + pose: Pose, name="attached_geom", art_name=None, link_id=-1, @@ -542,8 +525,7 @@ def update_attached_object( Attach given object (w/ collision geometry) to specified link of articulation :param collision_geometry: FCL collision geometry - :param pose: attaching pose (relative pose from attached link to object), - [x,y,z,qw,qx,qy,qz] + :param pose: attaching pose (relative pose from attached link to object) :param name: name of the attached geometry. :param art_name: name of the articulated object to attach to. If None, attach to self.robot. @@ -559,13 +541,14 @@ def update_attached_object( pose, ) - def update_attached_sphere(self, radius, pose, art_name=None, link_id=-1): + def update_attached_sphere( + self, radius: float, pose: Pose, art_name=None, link_id=-1 + ): """ Attach a sphere to some link :param radius: radius of the sphere - :param pose: attaching pose (relative pose from attached link to object), - [x,y,z,qw,qx,qy,qz] + :param pose: attaching pose (relative pose from attached link to object) :param art_name: name of the articulated object to attach to. If None, attach to self.robot. :param link_id: if not provided, the end effector will be the target. @@ -579,13 +562,19 @@ def update_attached_sphere(self, radius, pose, art_name=None, link_id=-1): pose, ) - def update_attached_box(self, size, pose, art_name=None, link_id=-1): + def update_attached_box( + self, + size: Sequence[float] + | np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], + pose: Pose, + art_name=None, + link_id=-1, + ): """ Attach a box to some link :param size: box side length - :param pose: attaching pose (relative pose from attached link to object), - [x,y,z,qw,qx,qy,qz] + :param pose: attaching pose (relative pose from attached link to object) :param art_name: name of the articulated object to attach to. If None, attach to self.robot. :param link_id: if not provided, the end effector will be the target. @@ -593,16 +582,20 @@ def update_attached_box(self, size, pose, art_name=None, link_id=-1): if link_id == -1: link_id = self.move_group_link_id self.planning_world.attach_box( - size, self.robot.get_name() if art_name is None else art_name, link_id, pose + size, # type: ignore + self.robot.get_name() if art_name is None else art_name, + link_id, + pose, ) - def update_attached_mesh(self, mesh_path, pose, art_name=None, link_id=-1): + def update_attached_mesh( + self, mesh_path: str, pose: Pose, art_name=None, link_id=-1 + ): """ Attach a mesh to some link :param mesh_path: path to a mesh file - :param pose: attaching pose (relative pose from attached link to object), - [x,y,z,qw,qx,qy,qz] + :param pose: attaching pose (relative pose from attached link to object) :param art_name: name of the articulated object to attach to. If None, attach to self.robot. :param link_id: if not provided, the end effector will be the target. @@ -627,12 +620,12 @@ def detach_object(self, name="attached_geom", also_remove=False) -> bool: """ return self.planning_world.detach_object(name, also_remove) - def set_base_pose(self, pose): + def set_base_pose(self, pose: Pose): """ tell the planner where the base of the robot is w.r.t the world frame Args: - pose: [x,y,z,qw,qx,qy,qz] pose of the base + pose: pose of the base """ self.robot.set_base_pose(pose) @@ -733,23 +726,13 @@ def plan_qpos_to_qpos( else: return {"status": f"RRTConnect Failed. {status}"} - def transform_goal_to_wrt_base(self, goal_pose): - base_pose = self.robot.get_base_pose() - base_tf = np.eye(4) - base_tf[0:3, 3] = base_pose[:3] - base_tf[0:3, 0:3] = quat2mat(base_pose[3:]) - goal_tf = np.eye(4) - goal_tf[0:3, 3] = goal_pose[:3] - goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:]) - goal_tf = np.linalg.inv(base_tf).dot(goal_tf) - new_goal_pose = np.zeros(7) - new_goal_pose[:3] = goal_tf[0:3, 3] - new_goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3]) - return new_goal_pose + def _transform_goal_to_wrt_base(self, goal_pose: Pose) -> Pose: + """Converts goal pose from T_world_goal to T_base_goal""" + return self.robot.get_base_pose().inv() * goal_pose def plan_qpos_to_pose( self, - goal_pose: np.ndarray, + goal_pose: Pose, current_qpos: np.ndarray, mask: Optional[list[bool] | np.ndarray] = None, *, @@ -768,7 +751,7 @@ def plan_qpos_to_pose( plan from a start configuration to a goal pose of the end-effector Args: - goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal + goal_pose: pose of the goal current_qpos: current joint configuration (either full or move_group joints) mask: if the value at a given index is True, the joint is *not* used in the IK @@ -791,7 +774,7 @@ def plan_qpos_to_pose( current_qpos = self.pad_move_group_qpos(current_qpos) if wrt_world: - goal_pose = self.transform_goal_to_wrt_base(goal_pose) + goal_pose = self._transform_goal_to_wrt_base(goal_pose) # we need to take only the move_group joints when planning # idx = self.move_group_joint_indices @@ -835,7 +818,7 @@ def plan_qpos_to_pose( # plan_screw ankor def plan_screw( self, - goal_pose: np.ndarray, + goal_pose: Pose, current_qpos: np.ndarray, *, qpos_step: float = 0.1, @@ -849,7 +832,7 @@ def plan_screw( screw motion Args: - goal_pose: [x, y, z, qw, qx, qy, qz] pose of the goal + goal_pose: pose of the goal current_qpos: current joint configuration (either full or move_group joints) qpos_step: size of the random step for RRT time_step: time step for the discretization @@ -861,13 +844,7 @@ def plan_screw( self.robot.set_qpos(current_qpos, True) if wrt_world: - goal_pose = self.transform_goal_to_wrt_base(goal_pose) - - def pose7D2mat(pose): - mat = np.eye(4) - mat[0:3, 3] = pose[:3] - mat[0:3, 0:3] = quat2mat(pose[3:]) - return mat + goal_pose = self._transform_goal_to_wrt_base(goal_pose) def skew(vec): return np.array([ @@ -876,7 +853,7 @@ def skew(vec): [-vec[1], vec[0], 0], ]) - def pose2exp_coordinate(pose: np.ndarray) -> tuple[np.ndarray, float]: + def pose2exp_coordinate(pose: Pose) -> tuple[np.ndarray, float]: def rot2so3(rotation: np.ndarray): assert rotation.shape == (3, 3) if np.isclose(rotation.trace(), 3): @@ -896,7 +873,8 @@ def rot2so3(rotation: np.ndarray): ) return omega, theta - omega, theta = rot2so3(pose[:3, :3]) + pose_mat = pose.to_transformation_matrix() + omega, theta = rot2so3(pose_mat[:3, :3]) if theta < -1e5: return omega, theta ss = skew(omega) @@ -905,16 +883,14 @@ def rot2so3(rotation: np.ndarray): - 0.5 * ss + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss ) - v = inv_left_jacobian @ pose[:3, 3] + v = inv_left_jacobian @ pose_mat[:3, 3] return np.concatenate([v, omega]), theta self.pinocchio_model.compute_forward_kinematics(current_qpos) ee_index = self.link_name_2_idx[self.move_group] - current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index)) - target_p = pose7D2mat(goal_pose) - relative_transform = target_p @ np.linalg.inv(current_p) - - omega, theta = pose2exp_coordinate(relative_transform) + # relative_pose = T_base_goal * T_base_link.inv() + relative_pose = goal_pose * self.pinocchio_model.get_link_pose(ee_index).inv() + omega, theta = pose2exp_coordinate(relative_pose) if theta < -1e4: return {"status": "screw plan failed."} diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 4de4d05..245adb0 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -12,6 +12,7 @@ __all__ = [ "ArticulatedModel", "AttachedBody", "PlanningWorld", + "Pose", "collision_detection", "kinematics", "planning", @@ -76,15 +77,11 @@ class ArticulatedModel: ``False``. :param verbose: print debug information. Default: ``False``. """ - def get_base_pose( - self, - ) -> numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ]: + def get_base_pose(self) -> Pose: """ - Get the base pose of the robot. + Get the base (root) pose of the robot. - :return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format + :return: base pose of the robot """ def get_fcl_model(self) -> collision_detection.fcl.FCLModel: """ @@ -128,6 +125,12 @@ class ArticulatedModel: :return: Pinocchio model used for kinematics and dynamics computations """ + def get_pose(self) -> Pose: + """ + Get the base (root) pose of the robot. + + :return: base pose of the robot + """ def get_qpos( self, ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]: @@ -148,16 +151,12 @@ class ArticulatedModel: :return: list of link names of the user """ - def set_base_pose( - self, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], - ) -> None: + def set_base_pose(self, pose: Pose) -> None: """ - Set the base pose of the robot. + Set the base pose of the robot and update all collision links in the + ``FCLModel``. - :param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format + :param pose: base pose of the robot """ @typing.overload def set_move_group(self, end_effector: str) -> None: @@ -181,13 +180,21 @@ class ArticulatedModel: @param: name of the articulated model """ + def set_pose(self, pose: Pose) -> None: + """ + Set the base pose of the robot and update all collision links in the + ``FCLModel``. + + :param pose: base pose of the robot + """ def set_qpos( self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], full: bool = False, ) -> None: """ - Let the planner know the current joint positions. + Set the current joint positions and update all collision links in the + ``FCLModel``. :param qpos: current qpos of all active joints or just the move group joints :param full: whether to set the full qpos or just the move group qpos. If full @@ -200,6 +207,34 @@ class ArticulatedModel: :param srdf: path to SRDF file, can be relative to the current working directory """ + @property + def base_pose(self) -> Pose: + """ + The base (root) pose of the robot + """ + @base_pose.setter + def base_pose(self, arg1: Pose) -> None: ... + @property + def name(self) -> str: + """ + Name of the articulated model + """ + @name.setter + def name(self, arg1: str) -> None: ... + @property + def pose(self) -> Pose: + """ + The base (root) pose of the robot + """ + @pose.setter + def pose(self, arg1: Pose) -> None: ... + @property + def qpos( + self, + ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]: + """ + Current qpos of all active joints + """ class AttachedBody: """ @@ -216,9 +251,7 @@ class AttachedBody: object: collision_detection.fcl.FCLObject, attached_articulation: ArticulatedModel, attached_link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: Pose, touch_links: list[str] = [], ) -> None: """ @@ -233,13 +266,17 @@ class AttachedBody: """ def get_attached_articulation(self) -> ArticulatedModel: """ - Gets the articulation this body is attached to + Gets the articulation that this body is attached to + """ + def get_attached_link_global_pose(self) -> Pose: + """ + Gets the global pose of the articulation link that this body is attached to """ def get_attached_link_id(self) -> int: """ - Gets the articulation this body is attached to + Gets the articulation link id that this body is attached to """ - def get_global_pose(self) -> ...: + def get_global_pose(self) -> Pose: """ Gets the global pose of the attached object """ @@ -251,7 +288,7 @@ class AttachedBody: """ Gets the attached object (``FCLObjectPtr``) """ - def get_pose(self) -> ...: + def get_pose(self) -> Pose: """ Gets the attached pose (relative pose from attached link to object) """ @@ -259,12 +296,7 @@ class AttachedBody: """ Gets the link names that the attached body touches """ - def set_pose( - self, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], - ) -> None: + def set_pose(self, pose: Pose) -> None: """ Sets the attached pose (relative pose from attached link to object) """ @@ -276,6 +308,13 @@ class AttachedBody: """ Updates the global pose of the attached object using current state """ + @property + def pose(self) -> Pose: + """ + The attached pose (relative pose from attached link to object) + """ + @pose.setter + def pose(self, arg1: Pose) -> None: ... class PlanningWorld: """ @@ -356,9 +395,7 @@ class PlanningWorld: ], art_name: str, link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: Pose, ) -> None: """ Attaches given box to specified link of articulation (auto touch_links) @@ -369,13 +406,7 @@ class PlanningWorld: :param pose: attached pose (relative pose from attached link to object) """ def attach_mesh( - self, - mesh_path: str, - art_name: str, - link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + self, mesh_path: str, art_name: str, link_id: int, pose: Pose ) -> None: """ Attaches given mesh to specified link of articulation (auto touch_links) @@ -420,14 +451,7 @@ class PlanningWorld: """ @typing.overload def attach_object( - self, - name: str, - art_name: str, - link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], - touch_links: list[str], + self, name: str, art_name: str, link_id: int, pose: Pose, touch_links: list[str] ) -> None: """ Attaches existing normal object to specified link of articulation at given pose. @@ -444,15 +468,7 @@ class PlanningWorld: planned articulation with given name does not exist """ @typing.overload - def attach_object( - self, - name: str, - art_name: str, - link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], - ) -> None: + def attach_object(self, name: str, art_name: str, link_id: int, pose: Pose) -> None: """ Attaches existing normal object to specified link of articulation at given pose. If the object is not currently attached, automatically sets touch_links as the @@ -475,9 +491,7 @@ class PlanningWorld: p_geom: collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: Pose, touch_links: list[str], ) -> None: """ @@ -499,9 +513,7 @@ class PlanningWorld: p_geom: collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: Pose, ) -> None: """ Attaches given object (w/ p_geom) to specified link of articulation at given @@ -517,13 +529,7 @@ class PlanningWorld: :param pose: attached pose (relative pose from attached link to object) """ def attach_sphere( - self, - radius: float, - art_name: str, - link_id: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + self, radius: float, art_name: str, link_id: int, pose: Pose ) -> None: """ Attaches given sphere to specified link of articulation (auto touch_links) @@ -729,6 +735,182 @@ class PlanningWorld: Set qpos of all planned articulations """ +class Pose: + """ + Pose stored as a unit quaternion and a position vector + + This struct is intended to be used only for interfacing with Python. Internally, + ``Pose`` is converted to and stored as ``Eigen::Isometry3`` which is used by all + computations. + """ + def __getstate__(self) -> tuple: ... + def __imul__(self, other: Pose) -> Pose: + """ + Overloading operator *= for ``Pose *= Pose`` + """ + @typing.overload + def __init__(self) -> None: + """ + Constructs a default Pose with p = (0,0,0) and q = (1,0,0,0) + """ + @typing.overload + def __init__( + self, + p: numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ] = ..., + q: numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] + ] = ..., + ) -> None: + """ + Constructs a Pose with given position and quaternion + + :param p: position, format: (x, y, z) + :param q: quaternion (can be unnormalized), format: (w, x, y, z) + """ + @typing.overload + def __init__( + self, + matrix: numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64] + ], + ) -> None: + """ + Constructs a Pose with given transformation matrix + (4x4 np.ndarray with np.float64 dtype) + + :param matrix: a 4x4 np.float64 np.ndarray transformation matrix + """ + @typing.overload + def __init__(self, obj: typing.Any) -> None: + """ + Constructs a Pose with given Python object that has ``p`` and ``q`` attributes + (e.g., ``sapien.Pose``) or a 4x4 np.ndarray transformation matrix. + + :param obj: a Pose-like object with ``p`` and ``q`` attributes or + a 4x4 np.ndarray transformation matrix + """ + @typing.overload + def __mul__( + self, + v: numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ], + ) -> numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ]: + """ + Overloading operator * for ``Pose * Vector3`` + """ + @typing.overload + def __mul__(self, other: Pose) -> Pose: + """ + Overloading operator * for ``Pose * Pose`` + """ + def __repr__(self) -> str: ... + def __setstate__(self, arg0: tuple) -> None: ... + def distance(self, other: Pose) -> float: + """ + Computes the distance between two poses by ``norm(p1.p - p2.p) + min(norm(p1.q - + p2.q), norm(p1.q + p2.q))`. + + The quaternion part has range [0, sqrt(2)]. + + :param other: the other pose + :return: the distance between the two poses + """ + def get_p( + self, + ) -> numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ]: + """ + Gets the position part of the Pose + + :return: position, format: (x, y, z) + """ + def get_q( + self, + ) -> numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] + ]: + """ + Gets the quaternion part of the Pose + + :return: quaternion, format: (w, x, y, z) + """ + def inv(self) -> Pose: + """ + Get the inserse Pose + + :return: the inverse Pose + """ + def set_p( + self, + p: numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ], + ) -> None: + """ + Sets the position part of the Pose + + :param p: position, format: (x, y, z) + """ + def set_q( + self, + q: numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] + ], + ) -> None: + """ + Sets the quaternion part of the Pose + + :param q: quaternion (can be unnormalized), format: (w, x, y, z) + """ + def to_transformation_matrix( + self, + ) -> numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64] + ]: + """ + Constructs a transformation matrix from this Pose + + :return: a 4x4 transformation matrix + """ + @property + def p( + self, + ) -> numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ]: + """ + Position part of the Pose (x, y, z) + """ + @p.setter + def p( + self, + arg0: numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ], + ) -> None: ... + @property + def q( + self, + ) -> numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] + ]: + """ + Quaternion part of the Pose (w, x, y, z) + """ + @q.setter + def q( + self, + arg1: numpy.ndarray[ + tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] + ], + ) -> None: ... + def set_global_seed(seed: int) -> None: """ Sets the global seed for MPlib (``std::srand()``, OMPL's RNG, and FCL's RNG). diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 5d39a68..c66336d 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -6,6 +6,8 @@ import typing import numpy +import mplib.pymp + __all__ = [ "BVHModel", "Box", @@ -224,39 +226,34 @@ class CollisionObject: geometry. """ def __init__( - self, - collision_geometry: CollisionGeometry, - position: numpy.ndarray[ - tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] - ] = ..., - quaternion: numpy.ndarray[ - tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64] - ] = ..., + self, collision_geometry: CollisionGeometry, pose: mplib.pymp.Pose = ... ) -> None: """ Construct a collision object with given collision geometry and transformation. :param collision_geometry: collision geometry of the object - :param position: position of the object - :param quaternion: quatenion of the object pose in [w, x, y, z] format + :param pose: pose of the object """ def get_collision_geometry(self) -> CollisionGeometry: ... - def get_rotation( - self, - ) -> numpy.ndarray[ - tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64] - ]: ... - def get_translation( - self, - ) -> numpy.ndarray[ - tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] - ]: ... - def set_transformation( - self, - arg0: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], - ) -> None: ... + def get_pose(self) -> mplib.pymp.Pose: + """ + Gets the current pose of the collision object in world + + :return: The current pose of the collision object + """ + def set_pose(self, pose: mplib.pymp.Pose) -> None: + """ + Sets the pose of the collision object in world + + :param pose: New pose of the collision object + """ + @property + def pose(self) -> mplib.pymp.Pose: + """ + Pose of the collision object in world + """ + @pose.setter + def pose(self, arg1: mplib.pymp.Pose) -> None: ... class CollisionRequest: def __init__( @@ -627,14 +624,7 @@ class FCLModel: :param names: list of link names in the order that you want to set. """ - def update_collision_objects( - self, - link_poses: list[ - numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ] - ], - ) -> None: + def update_collision_objects(self, link_poses: list[mplib.pymp.Pose]) -> None: """ Update the collision objects of the FCL model. @@ -652,23 +642,28 @@ class FCLObject: https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1FCLObject.html https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1World_1_1Object.html """ - @staticmethod @typing.overload - def __init__(*args, **kwargs) -> None: + def __init__(self, name: str) -> None: """ - Construct a new FCLObject with the given name and shapes + Construct a new FCLObject with the given name :param name: name of this FCLObject - :param pose: pose of this FCLObject. All shapes are relative to this pose - :param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr`` - :param shape_poses: relative poses from this FCLObject to each collision shape """ @typing.overload - def __init__(self, name: str) -> None: + def __init__( + self, + name: str, + pose: mplib.pymp.Pose, + shapes: list[CollisionObject], + shape_poses: list[mplib.pymp.Pose], + ) -> None: """ - Construct a new FCLObject with the given name + Construct a new FCLObject with the given name and shapes :param name: name of this FCLObject + :param pose: pose of this FCLObject. All shapes are relative to this pose + :param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr`` + :param shape_poses: relative poses from this FCLObject to each collision shape """ @property def name(self) -> str: @@ -676,12 +671,12 @@ class FCLObject: Name of this FCLObject """ @property - def pose(self) -> ...: + def pose(self) -> mplib.pymp.Pose: """ Pose of this FCLObject. All shapes are relative to this pose """ @property - def shape_poses(self) -> list[..., 3, 1, ...]: + def shape_poses(self) -> list[mplib.pymp.Pose]: """ Relative poses from this FCLObject to each collision shape """ diff --git a/mplib/pymp/kinematics/kdl.pyi b/mplib/pymp/kinematics/kdl.pyi index ee9c29a..b485781 100644 --- a/mplib/pymp/kinematics/kdl.pyi +++ b/mplib/pymp/kinematics/kdl.pyi @@ -6,6 +6,8 @@ import typing import numpy +import mplib.pymp + __all__ = ["KDLModel"] M = typing.TypeVar("M", bound=int) @@ -27,9 +29,7 @@ class KDLModel: self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], - goal_pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + goal_pose: mplib.pymp.Pose, ) -> tuple[ numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int ]: ... @@ -37,9 +37,7 @@ class KDLModel: self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], - goal_pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + goal_pose: mplib.pymp.Pose, ) -> tuple[ numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int ]: ... @@ -47,9 +45,7 @@ class KDLModel: self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], - goal_pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + goal_pose: mplib.pymp.Pose, q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], ) -> tuple[ @@ -60,11 +56,7 @@ class KDLModel: self, endpoints: list[str], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], - goal_poses: list[ - numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ] - ], + goal_poses: list[mplib.pymp.Pose], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], ) -> tuple[ diff --git a/mplib/pymp/kinematics/pinocchio.pyi b/mplib/pymp/kinematics/pinocchio.pyi index 0181ecd..fa9060f 100644 --- a/mplib/pymp/kinematics/pinocchio.pyi +++ b/mplib/pymp/kinematics/pinocchio.pyi @@ -6,6 +6,8 @@ import typing import numpy +import mplib.pymp + __all__ = ["PinocchioModel"] M = typing.TypeVar("M", bound=int) N = typing.TypeVar("N", bound=int) @@ -52,9 +54,7 @@ class PinocchioModel: def compute_IK_CLIK( self, index: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: mplib.pymp.Pose, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], mask: list[bool] = [], eps: float = 1e-05, @@ -73,7 +73,7 @@ class PinocchioModel: :param index: index of the link (in the order you passed to the constructor or the default order) - :param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] + :param pose: desired pose of the link :param q_init: initial joint configuration :param mask: if the value at a given index is ``True``, the joint is *not* used in the IK @@ -86,9 +86,7 @@ class PinocchioModel: def compute_IK_CLIK_JL( self, index: int, - pose: numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ], + pose: mplib.pymp.Pose, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], @@ -109,7 +107,7 @@ class PinocchioModel: :param index: index of the link (in the order you passed to the constructor or the default order) - :param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] + :param pose: desired pose of the link :param q_init: initial joint configuration :param q_min: minimum joint configuration :param q_max: maximum joint configuration @@ -320,17 +318,13 @@ class PinocchioModel: to the constructor or the default order :return: name of the links """ - def get_link_pose( - self, index: int - ) -> numpy.ndarray[ - tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64] - ]: + def get_link_pose(self, index: int) -> mplib.pymp.Pose: """ - Get the pose of the given link. + Get the pose of the given link in robot's base (root) frame. :param index: index of the link (in the order you passed to the constructor or the default order) - :return: pose of the link [x, y, z, qw, qx, qy, qz] + :return: pose of the link in robot's base (root) frame. """ def get_random_configuration( self, diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 70dd278..b3fdd22 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -3,7 +3,7 @@ from typing import Optional, Sequence import numpy as np -from sapien import Entity, Pose, Scene +from sapien import Entity, Scene from sapien.physx import ( PhysxArticulation, PhysxArticulationLinkComponent, @@ -19,7 +19,7 @@ from transforms3d.euler import euler2quat from ..planner import Planner -from ..pymp import ArticulatedModel, PlanningWorld +from ..pymp import ArticulatedModel, PlanningWorld, Pose from ..pymp.collision_detection.fcl import ( Box, BVHModel, @@ -27,6 +27,7 @@ CollisionObject, Convex, Cylinder, + FCLObject, Halfspace, Sphere, collide, @@ -39,7 +40,9 @@ class SapienPlanningWorld(PlanningWorld): def __init__( - self, sim_scene: Scene, planned_articulations: list[PhysxArticulation] = [] + self, + sim_scene: Scene, + planned_articulations: list[PhysxArticulation] = [], # noqa: B006 ): """ Creates an mplib.pymp.planning_world.PlanningWorld from a sapien.Scene. @@ -56,12 +59,12 @@ def __init__( urdf_str = export_kinematic_chain_urdf(articulation) srdf_str = export_srdf(articulation) - # Get all links with collision shapes at local_pose - collision_links = [] # [(link_name, [CollisionObject, ...]), ...] - for link in articulation.links: - col_objs = self.convert_sapien_col_shape(link) - if len(col_objs) > 0: - collision_links.append((link.name, col_objs)) + # Convert all links to FCLObject + collision_links = [ + (link.name, fcl_obj) + for link in articulation.links + if (fcl_obj := self.convert_physx_component(link)) is not None + ] articulated_model = ArticulatedModel.create_from_urdf_string( urdf_str, @@ -81,76 +84,62 @@ def __init__( for entity in actors: component = entity.find_component_by_type(PhysxRigidBaseComponent) - assert ( - component is not None - ), f"No PhysxRigidBaseComponent found in {entity.name}: {entity.components=}" - assert not isinstance( - component, PhysxArticulationLinkComponent - ), f"Component should not be PhysxArticulationLinkComponent: {component=}" + assert component is not None, ( + f"No PhysxRigidBaseComponent found in {entity.name}: " + f"{entity.components=}" + ) # Convert collision shapes at current global pose - col_objs = self.convert_sapien_col_shape(component) # type: ignore - # TODO: multiple collision shapes - assert len(col_objs) == 1, ( - f"Should only have 1 collision object, got {len(col_objs)} shapes for " - f"entity '{entity.name}'" + self.add_normal_object( + self.get_object_name(entity), + self.convert_physx_component(component), # type: ignore ) - self.add_normal_object(self.get_object_name(entity), col_objs[0]) - def update_from_simulation(self, *, update_attached_object: bool = True) -> None: """ - Updates planning_world articulations/objects pose with current Scene state + Updates PlanningWorld's articulations/objects pose with current Scene state. + Note that shape's local_pose is not updated. + If those are changed, please recreate a new SapienPlanningWorld instance. :param update_attached_object: whether to update the attached pose of all attached objects """ for articulation in self._sim_scene.get_all_articulations(): - # set_qpos to update poses - self.get_articulation(self.get_object_name(articulation)).set_qpos( - articulation.qpos # type: ignore - ) + if art := self.get_articulation(self.get_object_name(articulation)): + # set_qpos to update poses + art.set_qpos(articulation.qpos) # type: ignore + else: + raise RuntimeError( + f"Articulation {articulation.name} not found in PlanningWorld! " + "The scene might have changed since last update." + ) for entity in self._sim_scene.get_all_actors(): - component = entity.find_component_by_type(PhysxRigidBaseComponent) - assert ( - component is not None - ), f"No PhysxRigidBaseComponent found in {entity.name}: {entity.components=}" - assert not isinstance( - component, PhysxArticulationLinkComponent - ), f"Component should not be PhysxArticulationLinkComponent: {component=}" - - shapes = component.collision_shapes # type: ignore - # TODO: multiple collision shapes - assert len(shapes) == 1, ( - f"Should only have 1 collision shape, got {len(shapes)} shapes for " - f"entity '{entity.name}'" - ) - shape = shapes[0] - - pose: Pose = entity.pose * shape.local_pose - # NOTE: Convert poses for Capsule/Cylinder - if isinstance( - shape, (PhysxCollisionShapeCapsule, PhysxCollisionShapeCylinder) - ): - pose = pose * Pose(q=euler2quat(0, np.pi / 2, 0)) # type: ignore - - # handle attached object - if self.is_normal_object_attached(self.get_object_name(entity)): - attached_body = self.get_attached_object(self.get_object_name(entity)) - if update_attached_object: - parent_posevec = ( - attached_body.get_attached_articulation() - .get_pinocchio_model() - .get_link_pose(attached_body.get_attached_link_id()) + object_name = self.get_object_name(entity) + + # If entity is an attached object + if attached_body := self.get_attached_object(object_name): + if update_attached_object: # update attached pose + attached_body.pose = ( + attached_body.get_attached_link_global_pose().inv() + * entity.pose # type: ignore ) - parent_pose = Pose(parent_posevec[:3], parent_posevec[3:]) # type: ignore - pose = parent_pose.inv() * pose # new attached pose - attached_body.set_pose(np.hstack((pose.p, pose.q))) attached_body.update_pose() + elif fcl_obj := self.get_normal_object(object_name): + # Overwrite the object + self.add_normal_object( + object_name, + FCLObject( + object_name, + entity.pose, # type: ignore + fcl_obj.shapes, + fcl_obj.shape_poses, + ), + ) else: - self.get_normal_object(self.get_object_name(entity)).set_transformation( - np.hstack((pose.p, pose.q)) + raise RuntimeError( + f"Entity {entity.name} not found in PlanningWorld! " + "The scene might have changed since last update." ) @staticmethod @@ -170,65 +159,64 @@ def get_object_name(obj: PhysxArticulation | Entity) -> str: raise NotImplementedError(f"Unknown SAPIEN object type {type(obj)}") @staticmethod - def convert_sapien_col_shape( - component: PhysxRigidBaseComponent, - ) -> list[CollisionObject]: - """Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCL CollisionObject - Returns a list of collision_obj at their current poses. - - If the component is an articulation link, the returned collision_obj is at - the shape's local_pose. - Otherwise, the returned collision_obj is at the entity's global pose + def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: """ - shapes = component.collision_shapes - if len(shapes) == 0: - return [] + Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. + All shapes in the returned FCLObject are already set at their world poses. - # NOTE: MPlib currently only supports 1 collision shape per object - # TODO: multiple collision shapes - assert len(shapes) == 1, ( - f"Should only have 1 collision shape, got {len(shapes)} shapes for " - f"entity '{component.entity.name}'" - ) + :param comp: a SAPIEN physx.PhysxRigidBaseComponent. + :return: an FCLObject containing all collision shapes in the Physx component. + If the component has no collision shapes, return ``None``. + """ + shapes: list[CollisionObject] = [] + shape_poses: list[Pose] = [] + for shape in comp.collision_shapes: + shape_poses.append(shape.local_pose) # type: ignore + + if isinstance(shape, PhysxCollisionShapeBox): + c_geom = Box(side=shape.half_size * 2) + elif isinstance(shape, PhysxCollisionShapeCapsule): + c_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Capsule has x-axis along capsule height + # FCL Capsule has z-axis along capsule height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapeConvexMesh): + assert np.allclose( + shape.scale, 1.0 + ), f"Not unit scale {shape.scale}, need to rescale vertices?" + c_geom = Convex(vertices=shape.vertices, faces=shape.triangles) + elif isinstance(shape, PhysxCollisionShapeCylinder): + c_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Cylinder has x-axis along cylinder height + # FCL Cylinder has z-axis along cylinder height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapePlane): + raise NotImplementedError( + "Support for Plane collision is not implemented yet in mplib, " + "need fcl::Plane" + ) + elif isinstance(shape, PhysxCollisionShapeSphere): + c_geom = Sphere(radius=shape.radius) + elif isinstance(shape, PhysxCollisionShapeTriangleMesh): + c_geom = BVHModel() + c_geom.begin_model() + c_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) # type: ignore + c_geom.end_model() + else: + raise TypeError(f"Unknown shape type: {type(shape)}") + shapes.append(CollisionObject(c_geom)) - shape = shapes[0] - if isinstance(component, PhysxArticulationLinkComponent): # articulation link - pose = shape.local_pose - else: - pose = component.entity.pose * shape.local_pose - - if isinstance(shape, PhysxCollisionShapeBox): - collision_geom = Box(side=shape.half_size * 2) - elif isinstance(shape, PhysxCollisionShapeCapsule): - collision_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Capsule has x-axis along capsule height - # FCL Capsule has z-axis along capsule height - pose = pose * Pose(q=euler2quat(0, np.pi / 2, 0)) # type: ignore - elif isinstance(shape, PhysxCollisionShapeConvexMesh): - assert np.allclose( - shape.scale, 1.0 - ), f"Not unit scale {shape.scale}, need to rescale vertices?" - collision_geom = Convex(vertices=shape.vertices, faces=shape.triangles) - elif isinstance(shape, PhysxCollisionShapeCylinder): - collision_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Cylinder has x-axis along cylinder height - # FCL Cylinder has z-axis along cylinder height - pose = pose * Pose(q=euler2quat(0, np.pi / 2, 0)) # type: ignore - elif isinstance(shape, PhysxCollisionShapePlane): - raise NotImplementedError( - "Support for Plane collision is not implemented yet in mplib, " - "need fcl::Plane" - ) - elif isinstance(shape, PhysxCollisionShapeSphere): - collision_geom = Sphere(radius=shape.radius) - elif isinstance(shape, PhysxCollisionShapeTriangleMesh): - collision_geom = BVHModel() - collision_geom.begin_model() - collision_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) - collision_geom.end_model() - else: - raise TypeError(f"Unknown shape type: {type(shape)}") - return [CollisionObject(collision_geom, pose.p, pose.q)] # type: ignore + if len(shapes) == 0: + return None + + return FCLObject( + comp.name + if isinstance(comp, PhysxArticulationLinkComponent) + else SapienPlanningWorld.get_object_name(comp.entity), + comp.entity.pose, # type: ignore + shapes, + shape_poses, + ) class SapienPlanner(Planner): @@ -303,7 +291,10 @@ def __init__( def update_from_simulation(self, *, update_attached_object: bool = True) -> None: """ - Updates planning_world articulations/objects pose with current Scene state. + Updates PlanningWorld's articulations/objects pose with current Scene state. + Note that shape's local_pose is not updated. + If those are changed, please recreate a new SapienPlanningWorld instance. + Directly calls ``SapienPlanningWorld.update_from_simulation()`` :param update_attached_object: whether to update the attached pose of diff --git a/pybind/collision_detection/fcl/pybind_collision_common.cpp b/pybind/collision_detection/fcl/pybind_collision_common.cpp index 11b69f9..3abb498 100644 --- a/pybind/collision_detection/fcl/pybind_collision_common.cpp +++ b/pybind/collision_detection/fcl/pybind_collision_common.cpp @@ -9,7 +9,7 @@ #include "docstring/collision_detection/fcl/collision_common.h" #include "mplib/collision_detection/fcl/collision_common.h" -#include "mplib/types.h" +#include "mplib/utils/pose.h" #include "pybind_macros.hpp" namespace py = pybind11; @@ -25,23 +25,29 @@ void build_pyfcl_collision_common(py::module &m) { auto PyFCLObject = py::class_, std::shared_ptr>>( m, "FCLObject", DOC(mplib, collision_detection, fcl, FCLObject)); - // TODO(merge): mplib.Pose PyFCLObject .def(py::init(), py::arg("name"), DOC(mplib, collision_detection, fcl, FCLObject, FCLObject)) - .def(py::init &, + .def(py::init &, const std::vector> &, - const std::vector> &>(), + const std::vector> &>(), py::arg("name"), py::arg("pose"), py::arg("shapes"), py::arg("shape_poses"), DOC(mplib, collision_detection, fcl, FCLObject, FCLObject, 2)) .def_readonly("name", &FCLObject::name, DOC(mplib, collision_detection, fcl, FCLObject, name)) - .def_readonly("pose", &FCLObject::pose, - DOC(mplib, collision_detection, fcl, FCLObject, pose)) + .def_property_readonly( + "pose", [](const FCLObject &fcl_obj) { return Pose(fcl_obj.pose); }, + DOC(mplib, collision_detection, fcl, FCLObject, pose)) .def_readonly("shapes", &FCLObject::shapes, DOC(mplib, collision_detection, fcl, FCLObject, shapes)) - .def_readonly("shape_poses", &FCLObject::shape_poses, - DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)); + .def_property_readonly( + "shape_poses", + [](const FCLObject &fcl_obj) { + std::vector> ret; + for (const auto &pose : fcl_obj.shape_poses) ret.push_back(Pose(pose)); + return ret; + }, + DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)); // collide / distance functions m.def( diff --git a/pybind/collision_detection/fcl/pybind_fcl.cpp b/pybind/collision_detection/fcl/pybind_fcl.cpp index 8777313..fa36faf 100644 --- a/pybind/collision_detection/fcl/pybind_fcl.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl.cpp @@ -10,7 +10,7 @@ #include "docstring/collision_detection/fcl/fcl.h" #include "mplib/types.h" -#include "mplib/utils/conversion.h" +#include "mplib/utils/pose.h" #include "pybind_macros.hpp" namespace py = pybind11; @@ -247,25 +247,34 @@ void build_pyfcl(py::module &m) { DOC(fcl, OcTree, OcTree, 2)); // Collision Object = Geometry + Transformation - // TODO(merge): get_transformation auto PyCollisionObject = py::class_>( m, "CollisionObject", DOC(fcl, CollisionObject)); PyCollisionObject .def(py::init([](const std::shared_ptr &cgeom, - const Vector3 &p, const Vector4 &q) { - auto rot_mat = Quaternion {q(0), q(1), q(2), q(3)}.matrix(); - return CollisionObject(cgeom, rot_mat, p); + const Pose &pose) { + return CollisionObject(cgeom, pose.toIsometry()); }), - py::arg("collision_geometry"), py::arg("position") = Vector3 {0, 0, 0}, - py::arg("quaternion") = Vector4 {1, 0, 0, 0}, + py::arg("collision_geometry"), py::arg("pose") = Pose(), DOC(fcl, CollisionObject, CollisionObject)) .def("get_collision_geometry", &CollisionObject::collisionGeometry) - .def("get_translation", &CollisionObject::getTranslation) - .def("get_rotation", &CollisionObject::getRotation) - .def("set_transformation", [](CollisionObject &a, const Vector7 &pose) { - a.setTransform(toIsometry(pose)); - }); + .def_property( + "pose", + [](const CollisionObject &obj) { return Pose(obj.getTransform()); }, + [](CollisionObject &obj, const Pose &pose) { + obj.setTransform(pose.toIsometry()); + }, + DOC(fcl, CollisionObject, pose)) + .def( + "set_pose", + [](CollisionObject &obj, const Pose &pose) { + obj.setTransform(pose.toIsometry()); + }, + py::arg("pose"), DOC(fcl, CollisionObject, set_pose)) + .def( + "get_pose", + [](const CollisionObject &obj) { return Pose(obj.getTransform()); }, + DOC(fcl, CollisionObject, get_pose)); /********** narrowphase *******/ auto PyGJKSolverType = py::enum_(m, "GJKSolverType"); diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 8f0b962..e08fa77 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -10,7 +10,6 @@ #include "docstring/collision_detection/fcl/fcl_model.h" #include "mplib/collision_detection/fcl/fcl_model.h" -#include "mplib/types.h" #include "pybind_macros.hpp" namespace py = pybind11; @@ -58,9 +57,7 @@ void build_pyfcl_model(py::module &m) { DOC(mplib, collision_detection, fcl, FCLModelTpl, removeCollisionPairsFromSRDF)) - .def("update_collision_objects", - py::overload_cast> &>( - &FCLModel::updateCollisionObjects, py::const_), + .def("update_collision_objects", &FCLModel::updateCollisionObjects, py::arg("link_poses"), DOC(mplib, collision_detection, fcl, FCLModelTpl, updateCollisionObjects)) diff --git a/pybind/core/pybind_articulated_model.cpp b/pybind/core/pybind_articulated_model.cpp index 9c5dae2..e008fec 100644 --- a/pybind/core/pybind_articulated_model.cpp +++ b/pybind/core/pybind_articulated_model.cpp @@ -54,6 +54,8 @@ void build_pyarticulated_model(py::module &pymp) { py::arg("verbose") = false, DOC(mplib, ArticulatedModelTpl, createFromURDFString)) + .def_property("name", &ArticulatedModel::getName, &ArticulatedModel::setName, + DOC(mplib, ArticulatedModelTpl, name)) .def("get_name", &ArticulatedModel::getName, DOC(mplib, ArticulatedModelTpl, getName)) .def("set_name", &ArticulatedModel::setName, py::arg("name"), @@ -85,16 +87,30 @@ void build_pyarticulated_model(py::module &pymp) { .def("get_move_group_qpos_dim", &ArticulatedModel::getQposDim, DOC(mplib, ArticulatedModelTpl, getQposDim)) + .def_property_readonly("qpos", &ArticulatedModel::getQpos, + DOC(mplib, ArticulatedModelTpl, qpos)) .def("get_qpos", &ArticulatedModel::getQpos, DOC(mplib, ArticulatedModelTpl, getQpos)) .def("set_qpos", &ArticulatedModel::setQpos, py::arg("qpos"), py::arg("full") = false, DOC(mplib, ArticulatedModelTpl, setQpos)) + .def_property("base_pose", &ArticulatedModel::getBasePose, + &ArticulatedModel::setBasePose, + DOC(mplib, ArticulatedModelTpl, base_pose)) .def("get_base_pose", &ArticulatedModel::getBasePose, DOC(mplib, ArticulatedModelTpl, getBasePose)) .def("set_base_pose", &ArticulatedModel::setBasePose, py::arg("pose"), DOC(mplib, ArticulatedModelTpl, setBasePose)) + // pose is an alias of base_pose + .def_property("pose", &ArticulatedModel::getBasePose, + &ArticulatedModel::setBasePose, + DOC(mplib, ArticulatedModelTpl, base_pose)) + .def("get_pose", &ArticulatedModel::getBasePose, + DOC(mplib, ArticulatedModelTpl, getBasePose)) + .def("set_pose", &ArticulatedModel::setBasePose, py::arg("pose"), + DOC(mplib, ArticulatedModelTpl, setBasePose)) + .def("update_SRDF", &ArticulatedModel::updateSRDF, py::arg("SRDF"), DOC(mplib, ArticulatedModelTpl, updateSRDF)); } diff --git a/pybind/core/pybind_attached_body.cpp b/pybind/core/pybind_attached_body.cpp index d8962c0..86a5bb6 100644 --- a/pybind/core/pybind_attached_body.cpp +++ b/pybind/core/pybind_attached_body.cpp @@ -23,17 +23,9 @@ void build_pyattached_body(py::module &pymp) { auto PyAttachedBody = py::class_>( pymp, "AttachedBody", DOC(mplib, AttachedBodyTpl)); PyAttachedBody - .def(py::init([](const std::string &name, const FCLObjectPtr &object, - const ArticulatedModelPtr &attached_articulation, - int attached_link_id, const Vector7 &posevec, - const std::vector &touch_links) { - Isometry3 pose; - pose.linear() = - Quaternion(posevec[3], posevec[4], posevec[5], posevec[6]).matrix(); - pose.translation() = posevec.head(3); - return AttachedBody(name, object, attached_articulation, attached_link_id, - pose, touch_links); - }), + .def(py::init &, + const std::vector &>(), py::arg("name"), py::arg("object"), py::arg("attached_articulation"), py::arg("attached_link_id"), py::arg("pose"), py::arg("touch_links") = std::vector(), @@ -45,19 +37,33 @@ void build_pyattached_body(py::module &pymp) { DOC(mplib, AttachedBodyTpl, getAttachedArticulation)) .def("get_attached_link_id", &AttachedBody::getAttachedLinkId, DOC(mplib, AttachedBodyTpl, getAttachedLinkId)) - .def("get_pose", &AttachedBody::getPose, DOC(mplib, AttachedBodyTpl, getPose)) + + .def_property( + "pose", [](const AttachedBody &body) { return Pose(body.getPose()); }, + [](AttachedBody &body, const Pose &pose) { + body.setPose(pose.toIsometry()); + }, + DOC(mplib, AttachedBodyTpl, pose)) + .def( + "get_pose", [](const AttachedBody &body) { return Pose(body.getPose()); }, + DOC(mplib, AttachedBodyTpl, getPose)) .def( "set_pose", - [](AttachedBody &self, const Vector7 &posevec) { - Isometry3 pose; - pose.linear() = - Quaternion(posevec[3], posevec[4], posevec[5], posevec[6]).matrix(); - pose.translation() = posevec.head(3); - self.setPose(pose); + [](AttachedBody &body, const Pose &pose) { + body.setPose(pose.toIsometry()); }, py::arg("pose"), DOC(mplib, AttachedBodyTpl, setPose)) - .def("get_global_pose", &AttachedBody::getGlobalPose, - DOC(mplib, AttachedBodyTpl, getGlobalPose)) + .def( + "get_attached_link_global_pose", + [](const AttachedBody &body) { + return Pose(body.getAttachedLinkGlobalPose()); + }, + DOC(mplib, AttachedBodyTpl, getAttachedLinkGlobalPose)) + .def( + "get_global_pose", + [](const AttachedBody &body) { return Pose(body.getGlobalPose()); }, + DOC(mplib, AttachedBodyTpl, getGlobalPose)) + .def("update_pose", &AttachedBody::updatePose, DOC(mplib, AttachedBodyTpl, updatePose)) .def("get_touch_links", &AttachedBody::getTouchLinks, diff --git a/pybind/docstring/collision_detection/fcl/fcl.h b/pybind/docstring/collision_detection/fcl/fcl.h index 4f22b5d..9848041 100644 --- a/pybind/docstring/collision_detection/fcl/fcl.h +++ b/pybind/docstring/collision_detection/fcl/fcl.h @@ -353,8 +353,23 @@ R"doc( Construct a collision object with given collision geometry and transformation. :param collision_geometry: collision geometry of the object -:param position: position of the object -:param quaternion: quatenion of the object pose in [w, x, y, z] format)doc"; +:param pose: pose of the object)doc"; + +static const char *__doc_fcl_CollisionObject_pose = +R"doc( +Pose of the collision object in world)doc"; + +static const char *__doc_fcl_CollisionObject_set_pose = +R"doc( +Sets the pose of the collision object in world + +:param pose: New pose of the collision object)doc"; + +static const char *__doc_fcl_CollisionObject_get_pose = +R"doc( +Gets the current pose of the collision object in world + +:return: The current pose of the collision object)doc"; /* ----- End of custom docstring section ----- */ diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 78b30c9..2fd9f49 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -129,12 +129,6 @@ Update the collision objects of the FCL model. :param link_poses: list of link poses in the order of the link order)doc"; -static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_updateCollisionObjects_2 = -R"doc( -Update the collision objects of the FCL model. - -:param link_poses: list of link poses in the order of the link order)doc"; - /* ----- Begin of custom docstring section ----- */ /* ----- End of custom docstring section ----- */ diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index bf5099a..a71c3ce 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -64,9 +64,9 @@ Constructs an ArticulatedModel from URDF/SRDF strings and collision links static const char *__doc_mplib_ArticulatedModelTpl_getBasePose = R"doc( -Get the base pose of the robot. +Get the base (root) pose of the robot. -:return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format)doc"; +:return: base pose of the robot)doc"; static const char *__doc_mplib_ArticulatedModelTpl_getFCLModel = R"doc( @@ -130,9 +130,10 @@ Get the link names that the user has provided for planning. static const char *__doc_mplib_ArticulatedModelTpl_setBasePose = R"doc( -Set the base pose of the robot. +Set the base pose of the robot and update all collision links in the +``FCLModel``. -:param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format)doc"; +:param pose: base pose of the robot)doc"; static const char *__doc_mplib_ArticulatedModelTpl_setMoveGroup = R"doc( @@ -156,7 +157,8 @@ Set name of the articulated model. static const char *__doc_mplib_ArticulatedModelTpl_setQpos = R"doc( -Let the planner know the current joint positions. +Set the current joint positions and update all collision links in the +``FCLModel``. :param qpos: current qpos of all active joints or just the move group joints :param full: whether to set the full qpos or just the move group qpos. If full @@ -171,6 +173,15 @@ Update the SRDF file to disable self-collisions. /* ----- Begin of custom docstring section ----- */ +static const char *__doc_mplib_ArticulatedModelTpl_name = +R"doc(Name of the articulated model)doc"; + +static const char *__doc_mplib_ArticulatedModelTpl_qpos = +R"doc(Current qpos of all active joints)doc"; + +static const char *__doc_mplib_ArticulatedModelTpl_base_pose = +R"doc(The base (root) pose of the robot)doc"; + /* ----- End of custom docstring section ----- */ #if defined(__GNUG__) diff --git a/pybind/docstring/core/attached_body.h b/pybind/docstring/core/attached_body.h index dc0cc15..cd8316c 100644 --- a/pybind/docstring/core/attached_body.h +++ b/pybind/docstring/core/attached_body.h @@ -45,11 +45,15 @@ Construct an attached body for a specified link. static const char *__doc_mplib_AttachedBodyTpl_getAttachedArticulation = R"doc( -Gets the articulation this body is attached to)doc"; +Gets the articulation that this body is attached to)doc"; + +static const char *__doc_mplib_AttachedBodyTpl_getAttachedLinkGlobalPose = +R"doc( +Gets the global pose of the articulation link that this body is attached to)doc"; static const char *__doc_mplib_AttachedBodyTpl_getAttachedLinkId = R"doc( -Gets the articulation this body is attached to)doc"; +Gets the articulation link id that this body is attached to)doc"; static const char *__doc_mplib_AttachedBodyTpl_getGlobalPose = R"doc( @@ -85,6 +89,9 @@ Updates the global pose of the attached object using current state)doc"; /* ----- Begin of custom docstring section ----- */ +static const char *__doc_mplib_AttachedBodyTpl_pose = +R"doc(The attached pose (relative pose from attached link to object))doc"; + /* ----- End of custom docstring section ----- */ #if defined(__GNUG__) diff --git a/pybind/docstring/kinematics/pinocchio/pinocchio_model.h b/pybind/docstring/kinematics/pinocchio/pinocchio_model.h index fcf0b1e..fd8bc93 100644 --- a/pybind/docstring/kinematics/pinocchio/pinocchio_model.h +++ b/pybind/docstring/kinematics/pinocchio/pinocchio_model.h @@ -69,7 +69,7 @@ Compute the inverse kinematics using close loop inverse kinematics. :param index: index of the link (in the order you passed to the constructor or the default order) -:param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] +:param pose: desired pose of the link :param q_init: initial joint configuration :param mask: if the value at a given index is ``True``, the joint is *not* used in the IK @@ -86,7 +86,7 @@ the given limits. :param index: index of the link (in the order you passed to the constructor or the default order) -:param pose: desired pose of the link [x, y, z, qw, qx, qy, qz] +:param pose: desired pose of the link :param q_init: initial joint configuration :param q_min: minimum joint configuration :param q_max: maximum joint configuration @@ -275,11 +275,11 @@ Get the name of all the links. static const char *__doc_mplib_kinematics_pinocchio_PinocchioModelTpl_getLinkPose = R"doc( -Get the pose of the given link. +Get the pose of the given link in robot's base (root) frame. :param index: index of the link (in the order you passed to the constructor or the default order) -:return: pose of the link [x, y, z, qw, qx, qy, qz])doc"; +:return: pose of the link in robot's base (root) frame.)doc"; static const char *__doc_mplib_kinematics_pinocchio_PinocchioModelTpl_getModel = R"doc( diff --git a/pybind/docstring/utils/conversion.h b/pybind/docstring/utils/conversion.h index b6c9149..1a8d478 100644 --- a/pybind/docstring/utils/conversion.h +++ b/pybind/docstring/utils/conversion.h @@ -34,20 +34,12 @@ R"doc( static const char *__doc_mplib_toIsometry = R"doc( -Converts a pose_vec [px, py, pz, qw, qx, qy, qz] to an Eigen::Isometry3 matrix)doc"; - -static const char *__doc_mplib_toIsometry_2 = -R"doc( )doc"; -static const char *__doc_mplib_toIsometry_3 = +static const char *__doc_mplib_toIsometry_2 = R"doc( )doc"; -static const char *__doc_mplib_toPoseVec = -R"doc( -Converts an Eigen::Isometry3 matrix to a pose_vec [px, py, pz, qw, qx, qy, qz])doc"; - static const char *__doc_mplib_toSE3 = R"doc( )doc"; diff --git a/pybind/docstring/utils/pose.h b/pybind/docstring/utils/pose.h new file mode 100644 index 0000000..7f58434 --- /dev/null +++ b/pybind/docstring/utils/pose.h @@ -0,0 +1,147 @@ +#pragma once + +/* + This file contains docstrings for use in the Python bindings. + Do not edit! They were automatically extracted by mkdoc.py. + */ + +#define __EXPAND(x) x +#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT +#define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) +#define __CAT1(a, b) a ## b +#define __CAT2(a, b) __CAT1(a, b) +#define __DOC1(n1) __doc_##n1 +#define __DOC2(n1, n2) __doc_##n1##_##n2 +#define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 +#define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 +#define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 +#define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 +#define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 +#define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) + +#if defined(__GNUG__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif + +static const char *__doc_mplib_Pose = +R"doc(Pose stored as a unit quaternion and a position vector + +This struct is intended to be used only for interfacing with Python. Internally, +``Pose`` is converted to and stored as ``Eigen::Isometry3`` which is used by all +computations.)doc"; + +static const char *__doc_mplib_Pose_Pose = +R"doc( +Constructs a default Pose with p = (0,0,0) and q = (1,0,0,0))doc"; + +static const char *__doc_mplib_Pose_Pose_2 = +R"doc( +Constructs a Pose with given position and quaternion + +:param p: position, format: (x, y, z) +:param q: quaternion (can be unnormalized), format: (w, x, y, z))doc"; + +static const char *__doc_mplib_Pose_Pose_3 = +R"doc( +Constructs a Pose with given position and quaternion + +:param p: position, format: (x, y, z) +:param q: quaternion (can be unnormalized), format: (w, x, y, z))doc"; + +static const char *__doc_mplib_Pose_Pose_4 = +R"doc( +Constructs a Pose from a given ``Eigen::Isometry3`` instance + +:param pose: an ``Eigen::Isometry3`` instance)doc"; + +static const char *__doc_mplib_Pose_distance = +R"doc( +Computes the distance between two poses by ``norm(p1.p - p2.p) + min(norm(p1.q - +p2.q), norm(p1.q + p2.q))`. + +The quaternion part has range [0, sqrt(2)]. + +:param other: the other pose +:return: the distance between the two poses)doc"; + +static const char *__doc_mplib_Pose_inverse = +R"doc( +Get the inserse Pose + +:return: the inverse Pose)doc"; + +static const char *__doc_mplib_Pose_operator_imul = +R"doc( +Overloading operator *= for ``Pose *= Pose``)doc"; + +static const char *__doc_mplib_Pose_operator_mul = +R"doc( +Overloading operator * for ``Pose * Vector3``)doc"; + +static const char *__doc_mplib_Pose_operator_mul_2 = +R"doc( +Overloading operator * for ``Pose * Pose``)doc"; + +static const char *__doc_mplib_Pose_p = R"doc(Position part of the Pose (x, y, z))doc"; + +static const char *__doc_mplib_Pose_q = R"doc(Quaternion part of the Pose (w, x, y, z))doc"; + +static const char *__doc_mplib_Pose_toIsometry = +R"doc( +Converts the Pose to an ``Eigen::Isometry3`` instance + +:return: an ``Eigen::Isometry3`` instance)doc"; + +/* ----- Begin of custom docstring section ----- */ + +static const char *__doc_mplib_Pose_Pose_5 = +R"doc( +Constructs a Pose with given transformation matrix +(4x4 np.ndarray with np.float64 dtype) + +:param matrix: a 4x4 np.float64 np.ndarray transformation matrix)doc"; + +static const char *__doc_mplib_Pose_Pose_6 = +R"doc( +Constructs a Pose with given Python object that has ``p`` and ``q`` attributes +(e.g., ``sapien.Pose``) or a 4x4 np.ndarray transformation matrix. + +:param obj: a Pose-like object with ``p`` and ``q`` attributes or + a 4x4 np.ndarray transformation matrix)doc"; + +static const char *__doc_mplib_Pose_to_transformation_matrix = +R"doc( +Constructs a transformation matrix from this Pose + +:return: a 4x4 transformation matrix)doc"; + +static const char *__doc_mplib_Pose_set_p = +R"doc( +Sets the position part of the Pose + +:param p: position, format: (x, y, z))doc"; + +static const char *__doc_mplib_Pose_get_p = +R"doc( +Gets the position part of the Pose + +:return: position, format: (x, y, z))doc"; + +static const char *__doc_mplib_Pose_set_q = +R"doc( +Sets the quaternion part of the Pose + +:param q: quaternion (can be unnormalized), format: (w, x, y, z))doc"; + +static const char *__doc_mplib_Pose_get_q = +R"doc( +Gets the quaternion part of the Pose + +:return: quaternion, format: (w, x, y, z))doc"; + +/* ----- End of custom docstring section ----- */ + +#if defined(__GNUG__) +#pragma GCC diagnostic pop +#endif diff --git a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp index 263bb27..a8108b1 100644 --- a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp +++ b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp @@ -95,9 +95,20 @@ void build_pypinocchio_model(py::module &m) { py::arg("qpos"), DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, computeForwardKinematics)) - .def("get_link_pose", &PinocchioModel::getLinkPose, py::arg("index"), - DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getLinkPose)) - //.def("get_joint_pose", &PinocchioModel::getJointPose, py::arg("index")) + .def( + "get_link_pose", + [](const PinocchioModel &self, size_t index) { + return Pose(self.getLinkPose(index)); + }, + py::arg("index"), + DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getLinkPose)) + //.def( + // "get_joint_pose", + // [](const PinocchioModel &self, size_t index) { + // return Pose(self.getJointPose(index)); + // }, + // py::arg("index"), + // DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getJointPose)) .def("compute_full_jacobian", &PinocchioModel::computeFullJacobian, py::arg("qpos"), diff --git a/pybind/pybind.cpp b/pybind/pybind.cpp index 92cb77a..aa55679 100644 --- a/pybind/pybind.cpp +++ b/pybind/pybind.cpp @@ -26,9 +26,12 @@ void build_pyarticulated_model(py::module &pymp); void build_pyattached_body(py::module &pymp); void build_pyplanning_world(py::module &pymp); void build_utils_random(py::module &pymp); +void build_utils_pose(py::module &pymp); PYBIND11_MODULE(pymp, m) { m.doc() = "Motion planning python binding"; + // Need to be built first so other methods can use Pose() as default argument value + build_utils_pose(m); collision_detection::build_pycollision_detection(m); kinematics::build_pykinematics(m); diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 370e028..525078a 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -95,18 +95,18 @@ void build_pyplanning_world(py::module &pymp) { DOC(mplib, PlanningWorldTpl, attachObject, 2)) .def("attach_object", py::overload_cast &, const std::vector &>( + const Pose &, const std::vector &>( &PlanningWorld::attachObject), py::arg("name"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), py::arg("touch_links"), DOC(mplib, PlanningWorldTpl, attachObject, 3)) .def("attach_object", py::overload_cast &>(&PlanningWorld::attachObject), + const Pose &>(&PlanningWorld::attachObject), py::arg("name"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachObject, 4)) .def("attach_object", py::overload_cast &, + const std::string &, int, const Pose &, const std::vector &>( &PlanningWorld::attachObject), py::arg("name"), py::arg("p_geom"), py::arg("art_name"), py::arg("link_id"), @@ -114,7 +114,7 @@ void build_pyplanning_world(py::module &pymp) { DOC(mplib, PlanningWorldTpl, attachObject, 5)) .def("attach_object", py::overload_cast &>( + const std::string &, int, const Pose &>( &PlanningWorld::attachObject), py::arg("name"), py::arg("p_geom"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachObject, 6)) diff --git a/pybind/utils/pose.cpp b/pybind/utils/pose.cpp new file mode 100644 index 0000000..0a789b2 --- /dev/null +++ b/pybind/utils/pose.cpp @@ -0,0 +1,134 @@ +#include "mplib/utils/pose.h" + +#include +#include + +#include +#include +#include +#include + +#include "docstring/utils/pose.h" +#include "pybind_macros.hpp" + +namespace py = pybind11; + +namespace mplib { + +using Matrix4 = Eigen::Matrix; +using pyarray_t = py::array_t; + +void build_utils_pose(py::module &pymp) { + auto PyPose = + py::class_, std::shared_ptr>>(pymp, "Pose", DOC(mplib, Pose)); + + // Enable implicit conversions on the Python side from any py::object to Pose + py::implicitly_convertible>(); + + PyPose.def(py::init<>(), DOC(mplib, Pose, Pose)) + .def(py::init &, const Vector4 &>(), + py::arg("p") = Vector3 {0.0, 0.0, 0.0}, + py::arg("q") = Vector4 {1.0, 0.0, 0.0, 0.0}, DOC(mplib, Pose, Pose, 2)) + .def(py::init([](const Matrix4 &mat) { + return Pose {mat.block<3, 1>(0, 3), + Quaternion(mat.block<3, 3>(0, 0))}; + }), + py::arg("matrix"), DOC(mplib, Pose, Pose, 5)) + .def(py::init([](const py::object &obj) { + if (py::hasattr(obj, "p") && py::hasattr(obj, "q")) { + auto p = pyarray_t::ensure(obj.attr("p")); + auto q = pyarray_t::ensure(obj.attr("q")); + + if (!p || !q) + throw std::runtime_error( + "Failed to convert 'p' and 'q' to numpy arrays"); + + if (p.ndim() != 1 || p.size() != 3) + throw std::range_error("'p' must be 1D array of size 3"); + if (q.ndim() != 1 || q.size() != 4) + throw std::range_error("'q' must be 1D array of size 4"); + + return Pose(Vector3(static_cast(p.request().ptr)), + Vector4(static_cast(q.request().ptr))); + } + + auto mat = pyarray_t::ensure(obj); + if (!mat) + throw std::runtime_error( + "Unknown object type, cannot contruct a Pose instance from it!"); + + if (mat.ndim() != 2 || mat.shape(0) != 4 || mat.shape(1) != 4) + throw std::range_error("Input must be 2D array of shape (4, 4)"); + + auto eigen_mat = Matrix4(static_cast(mat.request().ptr)); + return Pose {eigen_mat.block<3, 1>(0, 3), + Quaternion(eigen_mat.block<3, 3>(0, 0))}; + }), + py::arg("obj"), DOC(mplib, Pose, Pose, 6)) + .def( + "to_transformation_matrix", + [](const Pose &pose) { + Matrix4 mat = Matrix4::Identity(); + mat.block<3, 3>(0, 0) = pose.q.toRotationMatrix(); + mat.block<3, 1>(0, 3) = pose.p; + return mat; + }, + DOC(mplib, Pose, to_transformation_matrix)) + + .def_readwrite("p", &Pose::p, DOC(mplib, Pose, p)) + .def( + "set_p", [](Pose &pose, const Vector3 &p) { pose.p = p; }, py::arg("p"), + DOC(mplib, Pose, set_p)) + .def( + "get_p", [](const Pose &pose) { return pose.p; }, DOC(mplib, Pose, get_p)) + + .def_property( + "q", + [](const Pose &pose) { + return Vector4 {pose.q.w(), pose.q.x(), pose.q.y(), pose.q.z()}; + }, + [](Pose &pose, const Vector4 &q) { + pose.q = Quaternion {q(0), q(1), q(2), q(3)}.normalized(); + }, + DOC(mplib, Pose, q)) + .def( + "set_q", + [](Pose &pose, const Vector4 &q) { + pose.q = Quaternion {q(0), q(1), q(2), q(3)}.normalized(); + }, + py::arg("q"), DOC(mplib, Pose, set_q)) + .def( + "get_q", + [](const Pose &pose) { + return Vector4 {pose.q.w(), pose.q.x(), pose.q.y(), pose.q.z()}; + }, + DOC(mplib, Pose, get_q)) + + .def("inv", &Pose::inverse, DOC(mplib, Pose, inverse)) + .def("distance", &Pose::distance, py::arg("other"), DOC(mplib, Pose, distance)) + .def(py::self * Vector3(), py::arg("v"), DOC(mplib, Pose, operator_mul)) + .def(py::self * py::self, py::arg("other"), DOC(mplib, Pose, operator_mul, 2)) + .def(py::self *= py::self, py::arg("other"), DOC(mplib, Pose, operator_imul)) + + .def("__repr__", + [](const Pose &pose) { + std::ostringstream oss; + oss << pose; + return oss.str(); + }) + .def(py::pickle( + [](const Pose &p) { // __getstate__ + return py::make_tuple(p.p(0), p.p(1), p.p(2), p.q.w(), p.q.x(), p.q.y(), + p.q.z()); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 7) { + throw std::runtime_error("Invalid state!"); + } + return Pose {{t[0].cast(), t[1].cast(), t[2].cast()}, + Quaternion {t[3].cast(), t[4].cast(), + t[5].cast(), t[6].cast()}}; + })); +} + +} // namespace mplib diff --git a/src/collision_detection/fcl/collision_common.cpp b/src/collision_detection/fcl/collision_common.cpp index a796026..e7c048e 100644 --- a/src/collision_detection/fcl/collision_common.cpp +++ b/src/collision_detection/fcl/collision_common.cpp @@ -18,15 +18,17 @@ DEFINE_TEMPLATE_FCL_COMMON(float); DEFINE_TEMPLATE_FCL_COMMON(double); template -FCLObject::FCLObject(const std::string &name, const Isometry3 &pose, - const std::vector> &shapes, - const std::vector> &shape_poses) - : name(name), pose(pose), shapes(shapes), shape_poses(shape_poses) { - ASSERT(shapes.size() == shape_poses.size(), +FCLObject::FCLObject(const std::string &name_, const Pose &pose_, + const std::vector> &shapes_, + const std::vector> &shape_poses_) + : name(name_), pose(pose_.toIsometry()), shapes(shapes_) { + ASSERT(shapes_.size() == shape_poses_.size(), "shapes and shape_poses should have the same size"); - // Update pose of the shapes - for (size_t i = 0; i < shapes.size(); i++) - shapes[i]->setTransform(pose * shape_poses[i]); + for (size_t i = 0; i < shapes_.size(); i++) { + shape_poses.push_back(shape_poses_[i].toIsometry()); + // Update pose of the shapes + shapes_[i]->setTransform(pose * shape_poses[i]); + } } template diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index fd3e843..194d8f2 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -225,21 +225,9 @@ void FCLModelTpl::removeCollisionPairsFromSRDFString( template void FCLModelTpl::updateCollisionObjects( - const std::vector> &link_poses) const { + const std::vector> &link_poses) const { for (size_t i = 0; i < collision_objects_.size(); i++) { - const auto link_pose = toIsometry(link_poses[collision_link_user_indices_[i]]); - const auto &fcl_obj = collision_objects_[i]; - fcl_obj->pose = link_pose; - for (size_t j = 0; j < fcl_obj->shapes.size(); j++) - fcl_obj->shapes[j]->setTransform(link_pose * fcl_obj->shape_poses[j]); - } -} - -template -void FCLModelTpl::updateCollisionObjects( - const std::vector> &link_poses) const { - for (size_t i = 0; i < collision_objects_.size(); i++) { - const auto link_pose = link_poses[collision_link_user_indices_[i]]; + const auto link_pose = link_poses[collision_link_user_indices_[i]].toIsometry(); const auto &fcl_obj = collision_objects_[i]; fcl_obj->pose = link_pose; for (size_t j = 0; j < fcl_obj->shapes.size(); j++) diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index de67ec3..a8a5129 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -36,7 +36,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, fcl_model_->removeCollisionPairsFromSRDF(srdf_filename); current_qpos_ = VectorX::Constant(pinocchio_model_->getModel().nv, 0); setMoveGroup(user_link_names_); - setBasePose({0, 0, 0, 1, 0, 0, 0}); // initialize base pose to identity + setBasePose(Pose()); // initialize base pose to identity } template @@ -124,19 +124,10 @@ void ArticulatedModelTpl::setQpos(const VectorX &qpos, bool full) { } pinocchio_model_->computeForwardKinematics(current_qpos_); if (verbose_) print_verbose("current_qpos ", current_qpos_); - std::vector> link_pose; - for (size_t i = 0; i < user_link_names_.size(); i++) { - const Vector7 pose_i = pinocchio_model_->getLinkPose(i); - link_pose.push_back(base_pose_ * toIsometry(pose_i)); - } + std::vector> link_pose; + for (size_t i = 0; i < user_link_names_.size(); i++) + link_pose.push_back(Pose(base_pose_ * pinocchio_model_->getLinkPose(i))); fcl_model_->updateCollisionObjects(link_pose); } -template -void ArticulatedModelTpl::setBasePose(const Vector7 &pose) { - base_pose_ = toIsometry(pose); - // we don't need to update Qpos, but this also updates fcl, which we need - setQpos(current_qpos_, true); -} - } // namespace mplib diff --git a/src/core/attached_body.cpp b/src/core/attached_body.cpp index 9254ce5..5311faa 100644 --- a/src/core/attached_body.cpp +++ b/src/core/attached_body.cpp @@ -11,14 +11,14 @@ DEFINE_TEMPLATE_ATTACHED_BODY(double); template AttachedBodyTpl::AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object, const ArticulatedModelPtr &attached_articulation, - int attached_link_id, const Isometry3 &pose, + int attached_link_id, const Pose &pose, const std::vector &touch_links) : name_(name), object_(object), attached_articulation_(attached_articulation), pinocchio_model_(attached_articulation->getPinocchioModel()), attached_link_id_(attached_link_id), - pose_(pose), + pose_(pose.toIsometry()), touch_links_(touch_links) { updatePose(); // updates global pose using link_pose and attached_pose } diff --git a/src/kinematics/kdl/kdl_model.cpp b/src/kinematics/kdl/kdl_model.cpp index 275eea7..324048d 100644 --- a/src/kinematics/kdl/kdl_model.cpp +++ b/src/kinematics/kdl/kdl_model.cpp @@ -48,15 +48,15 @@ KDLModelTpl::KDLModelTpl(const std::string &urdf_filename, template std::tuple, int> KDLModelTpl::chainIKLMA(size_t index, const VectorX &q0, - const Vector7 &pose) const { + const Pose &pose) const { KDL::Chain chain; Vector6 L {1, 1, 1, 0.01, 0.01, 0.01}; ASSERT(index < user_link_names_.size(), "link index out of bound"); tree_.getChain(tree_root_name_, user_link_names_[index], chain); - KDL::Frame frame_goal = - KDL::Frame(KDL::Rotation::Quaternion(pose[4], pose[5], pose[6], pose[3]), - KDL::Vector(pose[0], pose[1], pose[2])); + KDL::Frame frame_goal = KDL::Frame( + KDL::Rotation::Quaternion(pose.q.x(), pose.q.y(), pose.q.z(), pose.q.w()), + KDL::Vector(pose.p(0), pose.p(1), pose.p(2))); KDL::ChainIkSolverPos_LMA solver(chain, L); int n = chain.getNrOfJoints(); KDL::JntArray q_init(n); @@ -77,14 +77,14 @@ std::tuple, int> KDLModelTpl::chainIKLMA(size_t index, template std::tuple, int> KDLModelTpl::chainIKNR(size_t index, const VectorX &q0, - const Vector7 &pose) const { + const Pose &pose) const { KDL::Chain chain; ASSERT(index < user_link_names_.size(), "link index out of bound"); tree_.getChain(tree_root_name_, user_link_names_[index], chain); - KDL::Frame frame_goal = - KDL::Frame(KDL::Rotation::Quaternion(pose[4], pose[5], pose[6], pose[3]), - KDL::Vector(pose[0], pose[1], pose[2])); + KDL::Frame frame_goal = KDL::Frame( + KDL::Rotation::Quaternion(pose.q.x(), pose.q.y(), pose.q.z(), pose.q.w()), + KDL::Vector(pose.p(0), pose.p(1), pose.p(2))); KDL::ChainFkSolverPos_recursive fkpossolver(chain); KDL::ChainIkSolverVel_pinv ikvelsolver(chain); @@ -110,16 +110,16 @@ std::tuple, int> KDLModelTpl::chainIKNR(size_t index, template std::tuple, int> KDLModelTpl::chainIKNRJL(size_t index, const VectorX &q0, - const Vector7 &pose, + const Pose &pose, const VectorX &qmin, const VectorX &qmax) const { KDL::Chain chain; ASSERT(index < user_link_names_.size(), "link index out of bound"); tree_.getChain(tree_root_name_, user_link_names_[index], chain); - KDL::Frame frame_goal = - KDL::Frame(KDL::Rotation::Quaternion(pose[4], pose[5], pose[6], pose[3]), - KDL::Vector(pose[0], pose[1], pose[2])); + KDL::Frame frame_goal = KDL::Frame( + KDL::Rotation::Quaternion(pose.q.x(), pose.q.y(), pose.q.z(), pose.q.w()), + KDL::Vector(pose.p(0), pose.p(1), pose.p(2))); KDL::ChainFkSolverPos_recursive fkpossolver(chain); KDL::ChainIkSolverVel_pinv ikvelsolver(chain); @@ -151,7 +151,7 @@ std::tuple, int> KDLModelTpl::chainIKNRJL(size_t index, template std::tuple, int> KDLModelTpl::TreeIKNRJL( const std::vector endpoints, const VectorX &q0, - const std::vector> &poses, const VectorX &qmin, + const std::vector> &poses, const VectorX &qmin, const VectorX &qmax) const { KDL::TreeFkSolverPos_recursive fkpossolver(tree_); KDL::TreeIkSolverVel_wdls ikvelsolver(tree_, endpoints); @@ -166,11 +166,11 @@ std::tuple, int> KDLModelTpl::TreeIKNRJL( } KDL::Frames frames; - for (size_t i = 0; i < endpoints.size(); i++) { - frames[endpoints[i]] = KDL::Frame( - KDL::Rotation::Quaternion(poses[i][4], poses[i][5], poses[i][6], poses[i][3]), - KDL::Vector(poses[i][0], poses[i][1], poses[i][2])); - } + for (size_t i = 0; i < endpoints.size(); i++) + frames[endpoints[i]] = + KDL::Frame(KDL::Rotation::Quaternion(poses[i].q.x(), poses[i].q.y(), + poses[i].q.z(), poses[i].q.w()), + KDL::Vector(poses[i].p(0), poses[i].p(1), poses[i].p(2))); for (int i = 0; i < n; i++) q_init(i) = q0[joint_mapping_kdl_2_user_[i]]; diff --git a/src/kinematics/pinocchio/pinocchio_model.cpp b/src/kinematics/pinocchio/pinocchio_model.cpp index 0d8cdab..8487634 100644 --- a/src/kinematics/pinocchio/pinocchio_model.cpp +++ b/src/kinematics/pinocchio/pinocchio_model.cpp @@ -523,7 +523,7 @@ void PinocchioModelTpl::computeForwardKinematics(const VectorX &qpos) { } template -Vector7 PinocchioModelTpl::getLinkPose(size_t index) const { +Isometry3 PinocchioModelTpl::getLinkPose(size_t index) const { ASSERT(index < static_cast(link_index_user2pinocchio_.size()), "The link index is out of bound!"); const auto frame = link_index_user2pinocchio_[index]; @@ -532,21 +532,17 @@ Vector7 PinocchioModelTpl::getLinkPose(size_t index) const { const auto link2joint = model_.frames[frame].placement; const auto joint2world = data_.oMi[parent_joint]; const auto link2world = joint2world * link2joint; - const auto p = link2world.translation(); - const auto q = Quaternion(link2world.rotation()); - return Vector7 {p.x(), p.y(), p.z(), q.w(), q.x(), q.y(), q.z()}; + return toIsometry(link2world); } template -Vector7 PinocchioModelTpl::getJointPose(size_t index) const { +Isometry3 PinocchioModelTpl::getJointPose(size_t index) const { ASSERT(index < static_cast(joint_index_user2pinocchio_.size()), "The link index is out of bound!"); const auto frame = joint_index_user2pinocchio_[index]; const auto joint2world = data_.oMi[frame]; - const auto p = joint2world.translation(); - const auto q = Quaternion(joint2world.rotation()); - return Vector7 {p.x(), p.y(), p.z(), q.w(), q.x(), q.y(), q.z()}; + return toIsometry(joint2world); } template @@ -593,7 +589,7 @@ Matrix6X PinocchioModelTpl::computeSingleLinkJacobian(const VectorX &qp template std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIK( - size_t index, const Vector7 &pose, const VectorX &q_init, + size_t index, const Pose &pose, const VectorX &q_init, const std::vector &mask, double eps, int max_iter, double dt, double damp) { ASSERT(index < static_cast(link_index_user2pinocchio_.size()), "link index out of bound"); @@ -601,11 +597,7 @@ std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIK( const auto jointId = model_.frames[frameId].parent; const auto link2joint = model_.frames[frameId].placement; - pinocchio::SE3Tpl link_pose; - link_pose.translation({pose[0], pose[1], pose[2]}); - link_pose.rotation( - Quaternion {pose[3], pose[4], pose[5], pose[6]}.toRotationMatrix()); - + const pinocchio::SE3Tpl link_pose {pose.q, pose.p}; const pinocchio::SE3Tpl joint_pose = link_pose * link2joint.inverse(); VectorX q = qposUser2Pinocchio(q_init); // pinocchio::neutral(model); Matrix6X J(6, model_.nv); @@ -648,7 +640,7 @@ std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIK( template std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIKJL( - size_t index, const Vector7 &pose, const VectorX &q_init, + size_t index, const Pose &pose, const VectorX &q_init, const VectorX &q_min, const VectorX &q_max, double eps, int max_iter, double dt, double damp) { ASSERT(index < static_cast(link_index_user2pinocchio_.size()), @@ -657,11 +649,7 @@ std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIKJL( const auto jointId = model_.frames[frameId].parent; const auto link2joint = model_.frames[frameId].placement; - pinocchio::SE3Tpl link_pose; - link_pose.translation({pose[0], pose[1], pose[2]}); - link_pose.rotation( - Quaternion {pose[3], pose[4], pose[5], pose[6]}.toRotationMatrix()); - + const pinocchio::SE3Tpl link_pose {pose.q, pose.p}; const pinocchio::SE3Tpl joint_pose = link_pose * link2joint.inverse(); VectorX q = qposUser2Pinocchio(q_init); // pinocchio::neutral(model); const VectorX qmin = qposUser2Pinocchio(q_min); diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 391b625..9c93ef8 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -5,7 +5,6 @@ #include "mplib/collision_detection/collision_matrix.h" #include "mplib/collision_detection/fcl/fcl_utils.h" #include "mplib/macros/assert.h" -#include "mplib/utils/conversion.h" namespace mplib { @@ -94,9 +93,9 @@ template void PlanningWorldTpl::addNormalObject(const std::string &name, const CollisionObjectPtr &collision_object) { addNormalObject(name, std::make_shared( - name, collision_object->getTransform(), + name, Pose(collision_object->getTransform()), std::vector {collision_object}, - std::vector> {Isometry3::Identity()})); + std::vector> {Pose()})); } template @@ -126,10 +125,9 @@ void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const std::vector &touch_links) { const auto T_world_obj = normal_object_map_.at(name)->pose; - const auto T_world_link = toIsometry( - planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose( - link_id)); - attachObject(name, art_name, link_id, toPoseVec(T_world_link.inverse() * T_world_obj), + const auto T_world_link = + planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj), touch_links); } @@ -137,23 +135,21 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id) { const auto T_world_obj = normal_object_map_.at(name)->pose; - const auto T_world_link = toIsometry( - planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose( - link_id)); - attachObject(name, art_name, link_id, - toPoseVec(T_world_link.inverse() * T_world_obj)); + const auto T_world_link = + planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj)); } template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, - const Vector7 &pose, + const Pose &pose, const std::vector &touch_links) { auto obj = normal_object_map_.at(name); auto nh = attached_body_map_.extract(name); auto body = std::make_shared(name, obj, planned_articulation_map_.at(art_name), - link_id, toIsometry(pose), touch_links); + link_id, pose.toIsometry(), touch_links); if (!nh.empty()) { // Update acm_ to disallow collision between name and previous touch_links acm_->removeEntry(name, nh.mapped()->getTouchLinks()); @@ -168,11 +164,11 @@ void PlanningWorldTpl::attachObject(const std::string &name, template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, - const Vector7 &pose) { + const Pose &pose) { auto obj = normal_object_map_.at(name); auto nh = attached_body_map_.extract(name); auto body = std::make_shared( - name, obj, planned_articulation_map_.at(art_name), link_id, toIsometry(pose)); + name, obj, planned_articulation_map_.at(art_name), link_id, pose.toIsometry()); if (!nh.empty()) { body->setTouchLinks(nh.mapped()->getTouchLinks()); nh.mapped() = body; @@ -197,7 +193,7 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, const std::string &art_name, int link_id, - const Vector7 &pose, + const Pose &pose, const std::vector &touch_links) { removeNormalObject(name); addNormalObject(name, std::make_shared(p_geom)); @@ -208,7 +204,7 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, const std::string &art_name, int link_id, - const Vector7 &pose) { + const Pose &pose) { removeNormalObject(name); addNormalObject(name, std::make_shared(p_geom)); attachObject(name, art_name, link_id, pose); @@ -216,7 +212,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, template void PlanningWorldTpl::attachSphere(S radius, const std::string &art_name, - int link_id, const Vector7 &pose) { + int link_id, const Pose &pose) { // FIXME: Use link_name to avoid changes auto name = art_name + "_" + std::to_string(link_id) + "_sphere"; attachObject(name, std::make_shared>(radius), art_name, link_id, pose); @@ -224,7 +220,7 @@ void PlanningWorldTpl::attachSphere(S radius, const std::string &art_name, template void PlanningWorldTpl::attachBox(const Vector3 &size, const std::string &art_name, - int link_id, const Vector7 &pose) { + int link_id, const Pose &pose) { // FIXME: Use link_name to avoid changes auto name = art_name + "_" + std::to_string(link_id) + "_box"; attachObject(name, std::make_shared>(size), art_name, link_id, pose); @@ -233,7 +229,7 @@ void PlanningWorldTpl::attachBox(const Vector3 &size, const std::string &a template void PlanningWorldTpl::attachMesh(const std::string &mesh_path, const std::string &art_name, int link_id, - const Vector7 &pose) { + const Pose &pose) { // TODO(merge): Convex mesh? // FIXME: Use link_name to avoid changes auto name = art_name + "_" + std::to_string(link_id) + "_mesh"; @@ -262,7 +258,7 @@ template void PlanningWorldTpl::printAttachedBodyPose() const { for (const auto &[name, body] : attached_body_map_) std::cout << name << " global pose:\n" - << body->getGlobalPose().matrix() << std::endl; + << Pose(body->getGlobalPose()) << std::endl; } template diff --git a/src/utils/conversion.cpp b/src/utils/conversion.cpp index 6b6e45d..d1144e5 100644 --- a/src/utils/conversion.cpp +++ b/src/utils/conversion.cpp @@ -4,8 +4,6 @@ namespace mplib { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_CONVERSION(S) \ - template Vector7 toPoseVec(const Isometry3 &pose); \ - template Isometry3 toIsometry(const Vector7 &pose_vec); \ template Isometry3 toIsometry(const pinocchio::SE3Tpl &T); \ template Isometry3 toIsometry(const urdf::Pose &M); \ template pinocchio::SE3Tpl toSE3(const Isometry3 &T); \ @@ -16,26 +14,6 @@ namespace mplib { DEFINE_TEMPLATE_CONVERSION(float); DEFINE_TEMPLATE_CONVERSION(double); -template -Vector7 toPoseVec(const Isometry3 &pose) { - const auto q = Quaternion {pose.linear()}; - Vector7 pose_vec; - pose_vec.template head<3>() = pose.translation(); - pose_vec(3) = q.w(); - pose_vec.template tail<3>() = q.vec(); - return pose_vec; -} - -template -Isometry3 toIsometry(const Vector7 &pose_vec) { - Isometry3 ret; - // NOTE: The quaternion is required to be normalized, otherwise it's UB - ret.linear() = Quaternion {pose_vec[3], pose_vec[4], pose_vec[5], pose_vec[6]} - .toRotationMatrix(); - ret.translation() = pose_vec.template head<3>(); - return ret; -} - template Isometry3 toIsometry(const pinocchio::SE3Tpl &T) { Isometry3 ret; diff --git a/tests/test_articulation.py b/tests/test_articulation.py index db97f2c..ba4ea72 100644 --- a/tests/test_articulation.py +++ b/tests/test_articulation.py @@ -6,6 +6,7 @@ from transforms3d.quaternions import mat2quat, quat2mat import mplib +from mplib.pymp import Pose from mplib.pymp.collision_detection import fcl FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -70,8 +71,8 @@ def setUp(self): def test_get_base_pose(self): base_pose = self.robot.get_base_pose() - self.assertIsInstance(base_pose, np.ndarray) - self.assertTrue(np.allclose(base_pose, np.array([0, 0, 0, 1, 0, 0, 0]))) + self.assertIsInstance(base_pose, Pose) + self.assertEqual(base_pose.distance(Pose()), 0) def test_get_fcl_model(self): fcl_model = self.robot.get_fcl_model() @@ -133,8 +134,9 @@ def test_get_user_link_names(self): def test_set_base_pose(self): pose = np.arange(7) / 10.0 pose[3:] /= np.linalg.norm(pose[3:]) + pose = Pose(pose[:3], pose[3:]) self.robot.set_base_pose(pose) - self.assertTrue(np.allclose(self.robot.get_base_pose(), pose)) + self.assertAlmostEqual(self.robot.get_base_pose().distance(pose), 0, places=3) def test_update_SRDF(self): self.robot.update_SRDF("") # should not raise error diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 96123fa..8274622 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -40,9 +40,7 @@ def setUp(self): for i, link_name in enumerate(self.collision_link_names): link_idx = self.pinocchio_model.get_link_names().index(link_name) link_pose = self.pinocchio_model.get_link_pose(link_idx) - self.model.get_collision_objects()[i].shapes[0].set_transformation( - link_pose - ) + self.model.get_collision_objects()[i].shapes[0].set_pose(link_pose) def test_get_collision_objects(self): self.assertEqual( @@ -50,13 +48,9 @@ def test_get_collision_objects(self): ) for i, link_name in enumerate(self.collision_link_names): pinocchio_idx = self.pinocchio_model.get_link_names().index(link_name) - pos = self.model.get_collision_objects()[i].shapes[0].get_translation() - quat = mat2quat( - self.model.get_collision_objects()[i].shapes[0].get_rotation() - ) + fcl_pose = self.model.get_collision_objects()[i].shapes[0].get_pose() pinocchio_pose = self.pinocchio_model.get_link_pose(pinocchio_idx) - self.assertTrue(np.allclose(pos, pinocchio_pose[:3])) - self.assertAlmostEqual(abs(quat.dot(pinocchio_pose[3:])), 1) + self.assertAlmostEqual(fcl_pose.distance(pinocchio_pose), 0, places=3) def test_remove_collision_pairs_from_srdf(self): old_collision_pairs = self.model.get_collision_pairs() diff --git a/tests/test_pinocchio.py b/tests/test_pinocchio.py index 2d60830..69a3331 100644 --- a/tests/test_pinocchio.py +++ b/tests/test_pinocchio.py @@ -126,11 +126,11 @@ def get_numerical_jacobian(self, qpos, link_idx, epsilon=1e-3): self.model.compute_forward_kinematics(qpos - perturbation) pose_minus = self.model.get_link_pose(link_idx) - position_diff = pose_plus[:3] - pose_minus[:3] + position_diff = pose_plus.get_p() - pose_minus.get_p() jacobian[:3, i] = position_diff / (2 * epsilon) - orientation_plus = pose_plus[3:] - orientation_minus = pose_minus[3:] + orientation_plus = pose_plus.get_q() + orientation_minus = pose_minus.get_q() # get the difference quaternion orientation_diff = qmult(orientation_plus, qinverse(orientation_minus)) # get the axis-angle representation @@ -138,7 +138,7 @@ def get_numerical_jacobian(self, qpos, link_idx, epsilon=1e-3): jacobian[3:, i] = angle / (2 * epsilon) * axis self.model.compute_forward_kinematics(qpos) - curr_pos = self.model.get_link_pose(link_idx)[:3] + curr_pos = self.model.get_link_pose(link_idx).get_p() speed_due_to_rotation = np.cross(jacobian[3:, i], curr_pos) jacobian[:3, i] -= speed_due_to_rotation diff --git a/tests/test_planner.py b/tests/test_planner.py index 3c2ad6b..abf07e6 100644 --- a/tests/test_planner.py +++ b/tests/test_planner.py @@ -1,5 +1,6 @@ import os import unittest +from copy import deepcopy import numpy as np import trimesh @@ -7,6 +8,7 @@ import mplib from mplib import Planner +from mplib.pymp import Pose from mplib.pymp.collision_detection import fcl FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -21,7 +23,7 @@ class TestPlanner(unittest.TestCase): def setUp(self): self.planner = Planner(**PANDA_SPEC) - self.target_pose = [0.4, 0.3, 0.12, 0, 1, 0, 0] + self.target_pose = Pose([0.4, 0.3, 0.12], [0, 1, 0, 0]) self.init_qpos = np.array([0, 0.2, 0, -2.6, 0, 3.0, 0.8, 0, 0]) self.joint_limits = [ [-2.8973, 2.8973], @@ -50,7 +52,7 @@ def sample_pose(self): pos = np.random.uniform(-1, 1, size=3) quat = np.random.uniform(0, 1, size=4) quat /= np.linalg.norm(quat) - pose = np.concatenate([pos, quat]) + pose = Pose(pos, quat) return pose def test_planning_to_pose(self): @@ -60,8 +62,8 @@ def test_planning_to_pose(self): self.assertEqual(result_sampling["status"], "Success") last_qpos = result_sampling["position"][-1] self.planner.robot.set_qpos(last_qpos) - self.assertTrue( - np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-3) + self.assertAlmostEqual( + self.get_end_effector_pose().distance(self.target_pose), 0, places=3 ) def test_planning_to_qpos(self): @@ -85,8 +87,8 @@ def test_planning_screw(self): self.assertEqual(result_sampling["status"], "Success") last_qpos = result_sampling["position"][-1] self.planner.robot.set_qpos(last_qpos) - self.assertTrue( - np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-2) + self.assertAlmostEqual( + self.get_end_effector_pose().distance(self.target_pose), 0, places=1 ) def test_wrap_joint_limit(self, tolerance=1e-3): @@ -136,7 +138,7 @@ def test_env_collision(self): floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box # create a collision object for the floor, with a 10cm offset in the z direction floor_fcl_collision_object = fcl.CollisionObject( - floor, [0, 0, -0.1], [1, 0, 0, 0] + floor, Pose([0, 0, -0.1], [1, 0, 0, 0]) ) # update the planning world with the floor collision object self.planner.planning_world.add_normal_object( @@ -169,8 +171,8 @@ def test_IK(self): if status == "Success": for result_qpos in results: self.planner.robot.set_qpos(result_qpos, full=True) - self.assertTrue( - np.allclose(self.get_end_effector_pose(), pose, atol=1e-3) + self.assertAlmostEqual( + self.get_end_effector_pose().distance(pose), 0, places=3 ) self.assertGreaterEqual(num_success, expected_least_num_success) @@ -178,53 +180,42 @@ def test_IK(self): floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box # create a collision object for the floor, with a 10cm offset in the z direction floor_fcl_collision_object = fcl.CollisionObject( - floor, [0, 0, -0.1], [1, 0, 0, 0] + floor, Pose([0, 0, -0.1], [1, 0, 0, 0]) ) - status, _ = self.planner.IK([0.4, 0.3, -0.1, 0, 1, 0, 0], self.init_qpos) + + under_floor_target_pose = deepcopy(self.target_pose) + under_floor_target_pose.set_p([0.4, 0.3, -0.1]) + status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos) self.assertEqual(status, "Success") self.planner.planning_world.add_normal_object( "floor", floor_fcl_collision_object ) - status, _ = self.planner.IK([0.4, 0.3, -0.1, 0, 1, 0, 0], self.init_qpos) + status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos) self.assertNotEqual(status, "Success") def test_set_base_pose(self): - target_pose_tf = np.eye(4) - target_pose_tf[:3, 3] = self.target_pose[:3] - target_pose_tf[:3, :3] = quat2mat(self.target_pose[3:]) - for _ in range(10): base_pose = self.sample_pose() self.planner.set_base_pose(base_pose) - quat = base_pose[3:] - hom_mat = np.eye(4) - hom_mat[:3, :3] = quat2mat(quat) - hom_mat[:3, 3] = base_pose[:3] - transformed_target_tf = hom_mat @ target_pose_tf - transformed_target_pose = np.concatenate([ - transformed_target_tf[:3, 3], - mat2quat(transformed_target_tf[:3, :3]), - ]) - result_sampling = self.planner.plan_qpos_to_pose( - transformed_target_pose, self.init_qpos + base_pose * self.target_pose, self.init_qpos ) self.assertEqual(result_sampling["status"], "Success") last_qpos_sampling = result_sampling["position"][-1] self.planner.robot.set_qpos(last_qpos_sampling) - self.assertTrue( - np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-2) + self.assertAlmostEqual( + self.get_end_effector_pose().distance(self.target_pose), 0, places=2 ) result_screw = self.planner.plan_screw( - transformed_target_pose, self.init_qpos + base_pose * self.target_pose, self.init_qpos ) self.assertEqual(result_screw["status"], "Success") last_qpos_screw = result_screw["position"][-1] self.planner.robot.set_qpos(last_qpos_screw) - self.assertTrue( - np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-2) + self.assertAlmostEqual( + self.get_end_effector_pose().distance(self.target_pose), 0, places=1 ) result_sampling = self.planner.plan_qpos_to_pose( @@ -243,6 +234,7 @@ def add_point_cloud(self): def test_update_point_cloud(self): # use screw based planning. first succeeds but after point cloud obstacle fails pose = [0.7, 0, 0.12, 0, 1, 0, 0] + pose = Pose([0.7, 0, 0.12], [0, 1, 0, 0]) result_screw = self.planner.plan_screw(pose, self.init_qpos) self.assertEqual(result_screw["status"], "Success") @@ -254,14 +246,16 @@ def test_update_point_cloud(self): def test_update_attach(self): starting_qpos = [0, 0.48, 0, -1.48, 0, 1.96, 0.78] - target_pose = [0.4, 0.3, 0.33, 0, 1, 0, 0] + target_pose = Pose([0.4, 0.3, 0.33], [0, 1, 0, 0]) self.add_point_cloud() result_screw = self.planner.plan_screw(target_pose, starting_qpos) self.assertEqual(result_screw["status"], "Success") # now attach a box to the end effector and we should fail - self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) + self.planner.update_attached_box( + [0.04, 0.04, 0.12], Pose([0, 0, 0.14], [1, 0, 0, 0]) + ) result_screw = self.planner.plan_screw(target_pose, starting_qpos) self.assertNotEqual(result_screw["status"], "Success") @@ -289,7 +283,7 @@ def make_f(self): def f(x, out): self.planner.robot.set_qpos(x) eef_pose = self.get_end_effector_pose() - eef_z_axis = quat2mat(eef_pose[3:])[:, 2] + eef_z_axis = quat2mat(eef_pose.get_q())[:, 2] out[0] = -eef_z_axis[2] - 0.883 # maintain 28 degrees w.r.t. -z axis return f @@ -302,31 +296,19 @@ def j(x, out): ) rot_jac = jac[3:, self.planner.move_group_joint_indices] eef_pose = self.get_end_effector_pose() - eef_z_axis = quat2mat(eef_pose[3:])[:, 2] + eef_z_axis = quat2mat(eef_pose.get_q())[:, 2] for i in range(len(self.planner.move_group_joint_indices)): out[i] = np.cross(rot_jac[:, i], eef_z_axis).dot(np.array([0, 0, -1])) return j def test_constrained_planning(self, success_percentage=0.3): - constrained_init_pose = [ - 0.4, - 0.3, - 0.12, - 0.1710101, - -0.9698463, - 0.0301537, - -0.1710101, - ] - constrained_target_pose = [ - 0.6, - 0.1, - 0.44, - 0.1710101, - 0.9698463, - -0.0301537, - -0.1710101, - ] + constrained_init_pose = Pose( + [0.4, 0.3, 0.12], [0.1710101, -0.9698463, 0.0301537, -0.1710101] + ) + constrained_target_pose = Pose( + [0.6, 0.1, 0.44], [0.1710101, -0.9698463, 0.0301537, -0.1710101] + ) # first do an ik to find the init_qpos status, results = self.planner.IK(constrained_init_pose, self.init_qpos) self.assertEqual(status, "Success")