diff --git a/CMakeLists.txt b/CMakeLists.txt index e338297a..72a80ff7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ include_directories(${OMPL_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS}) include_directories("src") # store libries in a variable -set(LIBS ompl fcl pinocchio assimp orocos-kdl Boost::system Boost::filesystem urdfdom_model urdfdom_world) +set(LIBS ompl fcl assimp orocos-kdl Boost::system Boost::filesystem urdfdom_model urdfdom_world) file(GLOB_RECURSE PROJECT_SRC "src/*.h" "src/*.cpp" "src/*.hpp") add_library(mp STATIC ${PROJECT_SRC}) diff --git a/dev/build_wheels.sh b/dev/build_wheels.sh old mode 100644 new mode 100755 diff --git a/dev/docker_setup.sh b/dev/docker_setup.sh old mode 100644 new mode 100755 diff --git a/dev/generate_stub_and_doc.sh b/dev/generate_stub_and_doc.sh old mode 100644 new mode 100755 diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index 2751126a..465fabfd 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -1,6 +1,6 @@ import sapien.core as sapien -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class PlanningDemo(DemoSetup): diff --git a/mplib/examples/constrained_planning.py b/mplib/examples/constrained_planning.py index af2e9517..fa671649 100644 --- a/mplib/examples/constrained_planning.py +++ b/mplib/examples/constrained_planning.py @@ -3,7 +3,7 @@ import numpy as np import transforms3d -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class ConstrainedPlanningDemo(DemoSetup): diff --git a/mplib/examples/demo.py b/mplib/examples/demo.py index 4f8ff9c1..fdba6e54 100644 --- a/mplib/examples/demo.py +++ b/mplib/examples/demo.py @@ -1,6 +1,6 @@ import sapien.core as sapien -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class PlanningDemo(DemoSetup): diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py index 7cbb8973..27909b9a 100644 --- a/mplib/examples/demo_setup.py +++ b/mplib/examples/demo_setup.py @@ -4,7 +4,7 @@ from sapien.utils.viewer import Viewer import mplib - +import numpy as np class DemoSetup: """ diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index fbe5fc81..7ccca2a3 100644 --- a/mplib/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -2,7 +2,7 @@ import mplib -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class DetectCollisionDemo(DemoSetup): diff --git a/mplib/examples/moving_robot.py b/mplib/examples/moving_robot.py index 5346ad4c..02a933a6 100644 --- a/mplib/examples/moving_robot.py +++ b/mplib/examples/moving_robot.py @@ -1,6 +1,6 @@ import sapien.core as sapien -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class PlanningDemo(DemoSetup): @@ -54,14 +54,14 @@ def __init__(self): def demo(self): """ Same demo as demo.py. - Although we shifted everything, the poses have not changed because these are w.r.t. the robot base - Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner - the poses are specified with respect to the world + Since we shifted the robot, we also need to shift the poses by 1 meter in the x and y direction + Alternatively, we can keep the poses the same and tell the planner + the poses are specified with respect to the base of the robot """ poses = [ - [0.4, 0.3, 0.12, 0, 1, 0, 0], - [0.2, -0.3, 0.08, 0, 1, 0, 0], - [0.6, 0.1, 0.14, 0, 1, 0, 0], + [1.4, 1.3, 0.12, 0, 1, 0, 0], + [1.2, 0.7, 0.08, 0, 1, 0, 0], + [1.6, 1.1, 0.14, 0, 1, 0, 0], ] for i in range(3): pose = poses[i] @@ -81,14 +81,14 @@ def demo(self): pose[2] += 0.12 self.move_to_pose(pose) - # convert the poses to be w.r.t. the world + # convert the poses to be w.r.t. the base for pose in poses: - pose[0] += 1 - pose[1] += 1 + pose[0] -= 1 + pose[1] -= 1 # plan a path to the first pose result = self.planner.plan_qpos_to_pose( - poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=True + poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=False ) if result["status"] != "Success": print(result["status"]) diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index 8b56ffaa..0f87649e 100644 --- a/mplib/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -3,7 +3,7 @@ import mplib -from .demo_setup import DemoSetup +from mplib.examples.demo_setup import DemoSetup class PlanningDemo(DemoSetup): diff --git a/mplib/planner.py b/mplib/planner.py index eaef39cc..8e2dcfa9 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -594,6 +594,20 @@ def plan_qpos_to_qpos( else: return {"status": "RRT Failed. %s" % 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 plan_qpos_to_pose( self, goal_pose, @@ -606,7 +620,7 @@ def plan_qpos_to_pose( use_point_cloud=False, use_attach=False, verbose=False, - wrt_world=False, + wrt_world=True, planner_name="RRTConnect", no_simplification=False, constraint_function=None, @@ -638,16 +652,7 @@ def plan_qpos_to_pose( current_qpos[i] = self.joint_limits[i][1] - 1e-3 if wrt_world: - 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) - goal_pose[:3] = goal_tf[0:3, 3] - goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3]) + goal_pose = self.transform_goal_to_wrt_base(goal_pose) idx = ( self.move_group_joint_indices @@ -702,6 +707,7 @@ def plan_screw( use_point_cloud=False, use_attach=False, verbose=False, + wrt_world=True, ): """ plan from a start configuration to a goal pose of the end-effector using screw motion @@ -714,12 +720,16 @@ def plan_screw( use_point_cloud: if True, will use the point cloud to avoid collision use_attach: if True, will use the attached tool to avoid collision verbose: if True, will print the log of TOPPRA + wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame """ self.planning_world.set_use_point_cloud(use_point_cloud) self.planning_world.set_use_attach(use_attach) qpos = self.pad_qpos(qpos.copy()) self.robot.set_qpos(qpos, True) + if wrt_world: + target_pose = self.transform_goal_to_wrt_base(target_pose) + def pose7D2mat(pose): mat = np.eye(4) mat[0:3, 3] = pose[:3] diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index cdabf098..3ea3bb8b 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -45,7 +45,7 @@ ArticulatedModelTpl::ArticulatedModelTpl( fcl_model.removeCollisionPairsFromSrdf(srdf_filename); current_qpos = VectorX::Constant(pinocchio_model.getModel().nv, 0); setMoveGroup(user_link_names); - base_tf = Transform3::Identity(); + setBasePose({0, 0, 0, 1, 0, 0, 0}); // initialize base pose to identity } template