Merge pull request #55 from haosulab/misc-fixes

Misc fixes

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})

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):

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):

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):

mplib/examples/demo_setup.py

@@ -4,7 +4,7 @@
 from sapien.utils.viewer import Viewer
 
 import mplib
-
+import numpy as np
 
 class DemoSetup:
     """

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):

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"])

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):

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]

src/articulated_model.cpp

@@ -45,7 +45,7 @@ ArticulatedModelTpl<DATATYPE>::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 <typename DATATYPE>