prepare to the minor release

yijiangh · Oct 28, 2020 · a481072 · a481072
1 parent c52b2d6
commit a481072
Show file tree

Hide file tree

Showing 9 changed files with 95 additions and 60 deletions.
diff --git a/.bumpversion.cfg b/.bumpversion.cfg
@@ -4,14 +4,14 @@ message = Bump version to {new_version}
 commit = True
 tag = True
 
-[bumpversion:file:setup.py]
-search = version="{current_version}"
-replace = version="{new_version}"
+# [bumpversion:file:docs/conf.py]
+# search = version = release = '{current_version}'
+# replace = version = release = '{new_version}'
 
-[bumpversion:file:docsource/conf.py]
-search = release = "{current_version}"
-replace = release = "{new_version}"
+[bumpversion:file:CHANGELOG.md]
+search = Unreleased
+replace = {new_version}
 
 [bumpversion:file:src/compas_fab_pychoreo/__init__.py]
-search = __version__ = "{current_version}"
-replace = __version__ = "{new_version}"
+search = __version__ = '{current_version}'
+replace = __version__ = '{new_version}'
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -11,6 +11,13 @@ and this project adheres to `Semantic Versioning <https://semver.org/spec/v2.0.0
 Unreleased
 ----------
 
+**Changed**
+
+* Changed client and backend_features to use `PyChoreoX` instead of `PybulletX`
+
+0.2.0
+----------
+
 See [Issue #1](https://github.com/yijiangh/compas_fab_pychoreo/issues/1) for a visual summary.
 
 **Added**

diff --git a/README.md b/README.md
@@ -6,4 +6,15 @@
 
 An experimental package containing research prototpyes to integrate more planning functionalities to `compas_fab`.
 
+## Installation
+
+```bash
+# don't forget to activate conda environment if you are using one
+$ git clone --recursive https://github.com/yijiangh/compas_fab_pychoreo.git
+$ cd compas_fab_pychoreo
+$ git submodule update --init --recursive
+```
+
+## Documentation
+
 A development journal can be found [here](https://docs.google.com/document/d/1iWSzxXZkS72vfJJMGflXAZhpGFig4QMZsQWk8Bve9YA/edit?usp=sharing).
diff --git a/examples/itj/parsing.py b/examples/itj/parsing.py
@@ -36,13 +36,13 @@ def parse_collision_meshes_from_dir(dir_path, scale=1e-3):
 
 def rfl_setup():
     data_dir = os.path.abspath(os.path.join(HERE, "..", "..", "data", 'robots'))
+    # ! the original rfl.urdf use concave collision objects, over-conservative
     # urdf_filename = os.path.join(data_dir, 'rfl_description', 'rfl_description', "urdf", "rfl.urdf")
     urdf_filename = os.path.join(data_dir, 'rfl_description', 'rfl_description', "urdf", "rfl_pybullet.urdf")
     srdf_filename = os.path.join(data_dir, 'rfl_description', 'rfl_description', "urdf", "rfl.srdf")
     model = RobotModel.from_urdf_file(urdf_filename)
     semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
-    robot = Robot(model, semantics=semantics)
-    return urdf_filename, robot
+    return urdf_filename, semantics
 
 def itj_TC_PG500_cms():
     tc_dir_path = os.path.abspath(os.path.join(HERE, "data", 'itj_TC'))

diff --git a/examples/itj/run.py b/examples/itj/run.py
@@ -31,15 +31,16 @@
 from pybullet_planning import get_sample_fn, link_from_name, sample_tool_ik, interpolate_poses, get_joint_positions, pairwise_collision, \
     get_floating_body_collision_fn
 
-from compas_fab_pychoreo.backend_features.pybullet_configuration_collision_checker import PybulletConfigurationCollisionChecker
-from compas_fab_pychoreo.client import PyBulletClient
+from compas_fab_pychoreo.backend_features.pychoreo_configuration_collision_checker import PyChoreoConfigurationCollisionChecker
+from compas_fab_pychoreo.client import PyChoreoClient
 from compas_fab_pychoreo.conversions import pose_from_frame, frame_from_pose
 from compas_fab_pychoreo_examples.ik_solver import ik_abb_irb4600_40_255, InverseKinematicsSolver, get_ik_fn_from_ikfast
 from compas_fab_pychoreo.utils import divide_list_chunks, values_as_list
 
-from parsing import rfl_setup, itj_TC_PG500_cms, itj_TC_PG1000_cms, itj_rfl_obstacle_cms, itj_rfl_pipe_cms
-from utils import to_rlf_robot_full_conf, rfl_camera, notify, R11_START_CONF_VALS, R12_START_CONF_VALS
+from .parsing import rfl_setup, itj_TC_PG500_cms, itj_TC_PG1000_cms, itj_rfl_obstacle_cms, itj_rfl_pipe_cms
+from .utils import to_rlf_robot_full_conf, rfl_camera, notify, R11_START_CONF_VALS, R12_START_CONF_VALS
 
+# unit conversion
 MIL2M = 1e-3
 
 # RETREAT_DISTANCE = 0.05
@@ -69,34 +70,19 @@
 JSON_OUT_DIR = os.path.join(HERE, 'results')
 
 R11_INTER_CONF_VALS = [21000.0, 0.0, -4900.0, 0.0, -80.0, 65.0, 65.0, 20.0, -20.0]
-# R12_INTER_CONF_VALS = [-11753.0, -4915.0, 0.0, -45.0, 16.0, 10.0, 19.0, 0.0]
 R12_INTER_CONF_VALS = [-4056.0883789999998, -4000.8486330000001, 0.0, -22.834741999999999, -30.711554, 0.0, 57.335655000000003, 0.0]
+# | [-11753.0, -4915.0, 0.0, -45.0, 16.0, 10.0, 19.0, 0.0]
 
 def convert_r12_mil_deg(conf_vals, scale=MIL2M):
+    # convert degree to radians for revoluted joints
     return [conf_vals[i]*scale for i in range(0, 2)] + [rad(conf_vals[i]) for i in range(2, 8)]
 
 def compute_movement(json_path_in=JSON_PATH_IN, json_out_dir=JSON_OUT_DIR, viewer=False, debug=False, write=False, \
     seq_i=1, parse_transfer_motion=False, disable_attachment=False, disable_env=False):
     # * Connect to path planning backend and initialize robot parameters
-    urdf_filename, robot = rfl_setup()
+    urdf_filename, semantics = rfl_setup()
 
     arm_move_group = 'robot12'
-    ik_base_link_name = robot.get_base_link_name(group=arm_move_group)
-    ik_joint_names = robot.get_configurable_joint_names(group=arm_move_group)
-    ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
-    tool_link_name = robot.get_end_effector_link_name(group=arm_move_group)
-
-    yzarm_move_group = 'robot12_eaYZ'
-    yzarm_joint_names = robot.get_configurable_joint_names(group=yzarm_move_group)
-    yzarm_joint_types = robot.get_joint_types_by_names(yzarm_joint_names)
-
-    gantry_x_joint_name = 'bridge1_joint_EA_X'
-    gantry_joint_names = []
-    for jt_n, jt_t in zip(yzarm_joint_names, yzarm_joint_types):
-        if jt_t == 2:
-            gantry_joint_names.append(jt_n)
-    flange_link_name = robot.get_end_effector_link_name(group=yzarm_move_group)
-
     # * Load process from file
     with open(json_path_in, 'r') as f:
         json_str = f.read()
@@ -125,19 +111,38 @@ def compute_movement(json_path_in=JSON_PATH_IN, json_out_dir=JSON_OUT_DIR, viewe
     attached_cm_pipe2 = AttachedCollisionMesh(pipe_cms[0], 'robot12_link_2', ['robot12_link_2'])
     attached_cm_pipe3 = AttachedCollisionMesh(pipe_cms[1], 'robot12_link_3', ['robot12_link_3'])
 
-    with PyBulletClient(viewer=viewer) as client:
-        client.load_robot_from_urdf(urdf_filename)
-        client.compas_fab_robot = robot
-        robot_uid = client.robot_uid
-
-        # joint indices
+    with PyChoreoClient(viewer=viewer) as client:
+        robot = client.load_robot_from_urdf(urdf_filename)
+        robot.semantics = semantics
+        # robot's unique body index in pybullet
+        robot_uid = client.get_robot_pybullet_uid(robot)
+
+        # * link/joint info
+        ik_base_link_name = robot.get_base_link_name(group=arm_move_group)
+        ik_joint_names = robot.get_configurable_joint_names(group=arm_move_group)
+        ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
+        tool_link_name = robot.get_end_effector_link_name(group=arm_move_group)
+
+        yzarm_move_group = 'robot12_eaYZ'
+        yzarm_joint_names = robot.get_configurable_joint_names(group=yzarm_move_group)
+        yzarm_joint_types = robot.get_joint_types_by_names(yzarm_joint_names)
+
+        gantry_x_joint_name = 'bridge1_joint_EA_X'
+        gantry_joint_names = []
+        for jt_n, jt_t in zip(yzarm_joint_names, yzarm_joint_types):
+            if jt_t == 2:
+                gantry_joint_names.append(jt_n)
+        flange_link_name = robot.get_end_effector_link_name(group=yzarm_move_group)
+
+        # * joint indices in pybullet
         gantry_x_joint = joint_from_name(robot_uid, gantry_x_joint_name)
         gantry_joints = joints_from_names(robot_uid, gantry_joint_names)
         ik_base_link = link_from_name(robot_uid, ik_base_link_name)
         ik_joints = joints_from_names(robot_uid, ik_joint_names)
         ik_tool_link = link_from_name(robot_uid, tool_link_name)
         yzarm_joints = joints_from_names(robot_uid, yzarm_joint_names)
 
+        # * draw base frame and locate camera in pybullet
         draw_pose(unit_pose(), length=1.)
         cam = rfl_camera()
         set_camera_pose(cam['target'], cam['location'])
@@ -149,21 +154,24 @@ def compute_movement(json_path_in=JSON_PATH_IN, json_out_dir=JSON_OUT_DIR, viewe
 
         # * attachments
         for ee_acm in ee_acms:
-            client.add_attached_collision_mesh(ee_acm, YELLOW)
+            # ! note that options must contain 'robot' and 'mass' entries
+            client.add_attached_collision_mesh(ee_acm, options={'robot': robot, 'mass': 1, 'color': YELLOW})
         # pipe attachments
         # if not disable_attachment:
-        #     client.add_attached_collision_mesh(attached_cm_pipe2, BLUE)
-        #     client.add_attached_collision_mesh(attached_cm_pipe3, BLUE)
+        #     client.add_attached_collision_mesh(attached_cm_pipe2, options={'robot': robot, 'mass': 1, 'color': BLUE})
+        #     client.add_attached_collision_mesh(attached_cm_pipe3, options={'robot': robot, 'mass': 1, 'color': BLUE})
 
         # # * Add static collision mesh to planning scene
         if not disable_env:
             for o_cm in itj_rfl_obstacle_cms():
-                client.add_collision_mesh(o_cm, GREY)
+                client.add_collision_mesh(o_cm, {'color':GREY})
 
         # * collision sanity check
-        # start_confval = np.hstack([r12_start_conf_vals[:2]*1e-3, np.radians(r12_start_conf_vals[2:])])
-        # conf = Configuration(values=start_confval.tolist(), types=yzarm_joint_types, joint_names=yzarm_joint_names)
-        # assert not client.configuration_in_collision(conf, group=yzarm_move_group, options={'diagnosis':True})
+        start_confval = np.hstack([R12_START_CONF_VALS[:2]*1e-3, np.radians(R12_START_CONF_VALS[2:])])
+        conf = Configuration(values=start_confval.tolist(), types=yzarm_joint_types, joint_names=yzarm_joint_names)
+        assert not client.check_collisions(robot, conf, options={'diagnosis':True})
+        cprint('Valid initial conf.')
+        wait_if_gui()
 
         # * Individual process path planning
 
@@ -285,7 +293,7 @@ def sample_ik_fn(world_from_tcp):
         # sanity check, is beam colliding with the obstacles?
         with WorldSaver():
             env_obstacles = values_as_list(client.collision_objects)
-            for attachment in client.attachments.values():
+            for attachment in client.pychoreo_attachments.values():
                 body_collision_fn = get_floating_body_collision_fn(attachment.child, obstacles=env_obstacles)
 
                 inclamp_approach_pose = body_from_end_effector(cart_key_poses[0], attachment.grasp_pose)
@@ -485,7 +493,7 @@ def sample_ik_fn(world_from_tcp):
                 # set_joint_positions(robot_uid, traj_joints, traj_pt.scaled(MIL2M).values)
                 set_joint_positions(robot_uid, traj_joints, traj_pt.values)
                 traj_pt.scale(1/MIL2M)
-                for _, attach in client.attachments.items():
+                for _, attach in client.pychoreo_attachments.items():
                     attach.assign()
                 if debug:
                     wait_if_gui('sim transit.')

diff --git a/setup.py b/setup.py
@@ -16,9 +16,10 @@
     # Until COMPAS reaches 1.0, we pin major.minor and allow patch version updates
     'compas>=0.16.9,<0.17',
     'compas_fab>=0.12.0',
-    # 'compas_fab @ git+https://[email protected]/compas-dev/compas_fab.git@5163c5619d12e102f5e070731c7f21c139eb4549#egg=compas_fab-0.11.1',
-    'pybullet_planning>=0.5.0',
+    'pybullet_planning>=0.5.1',
+    'ikfast_pybind>=0.1.0'
 ]
+# 'compas_fab @ git+https://[email protected]/compas-dev/compas_fab.git@5163c5619d12e102f5e070731c7f21c139eb4549#egg=compas_fab-0.11.1',
 
 here = path.abspath(path.dirname(__file__))
 

diff --git a/src/compas_fab_pychoreo/client.py b/src/compas_fab_pychoreo/client.py
@@ -1,6 +1,6 @@
 from itertools import combinations
 from pybullet_planning import HideOutput
-from pybullet_planning import BASE_LINK, RED, GREEN
+from pybullet_planning import BASE_LINK, RED, GREEN, GREY
 from pybullet_planning import get_link_pose, link_from_name, get_disabled_collisions
 from pybullet_planning import set_joint_positions
 from pybullet_planning import joints_from_names
@@ -13,6 +13,7 @@
 from compas_fab.backends import PyBulletClient
 from compas_fab_pychoreo.planner import PyChoreoPlanner
 from compas_fab_pychoreo.utils import is_valid_option
+from compas_fab.backends.pybullet.const import STATIC_MASS
 
 from .exceptions import CollisionError
 from .exceptions import InverseKinematicsError
@@ -51,6 +52,14 @@ def load_robot(self, urdf_file):
 
     ###########################################################
 
+    def add_collision_mesh(self, collision_mesh, options=None):
+        # add color
+        self.planner.add_collision_mesh(collision_mesh, options=options)
+        color = is_valid_option(options, 'color', GREY)
+        name = collision_mesh.id
+        for body in self.collision_objects[name]:
+            set_color(body, color)
+
     def add_attached_collision_mesh(self, attached_collision_mesh, options=None):
         """Adds an attached collision object to the planning scene.
 
@@ -76,6 +85,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None):
         ee_link_pose = get_link_pose(robot_uid, tool_attach_link)
 
         body = attached_constr_info[0].body_id
+        mass = is_valid_option(options, 'mass', STATIC_MASS)
         color = is_valid_option(options, 'color', GREEN)
 
         # * update attachment collision links

diff --git a/tests/test_pb_client.py b/tests/test_pb_client.py
@@ -17,7 +17,7 @@
     joints_from_names, quat_angle_between, get_collision_fn, create_obj, unit_pose
 from pybullet_planning import wait_if_gui, wait_for_duration, remove_all_debug
 from pybullet_planning import plan_cartesian_motion, plan_cartesian_motion_lg
-from pybullet_planning import randomize, elapsed_time
+from pybullet_planning import randomize, elapsed_time, BLUE, GREEN, RED
 
 import ikfast_abb_irb4600_40_255
 
@@ -269,12 +269,12 @@ def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_PG500_cms, itj_beam_cm, co
 
         vals = [-1.4660765716752369, -0.22689280275926285, 0.27925268031909273, 0.17453292519943295, 0.22689280275926285, -0.22689280275926285]
         start_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names)
-        # client.set_robot_configuration(robot, start_conf, group=move_group)
+        # client.set_robot_configuration(robot, start_conf)
         # wait_if_gui()
 
         vals = [0.05235987755982989, -0.087266462599716474, -0.05235987755982989, 1.7104226669544429, 0.13962634015954636, -0.43633231299858238]
         end_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names)
-        # client.set_robot_configuration(robot, end_conf, group=move_group)
+        # client.set_robot_configuration(robot, end_conf)
         # wait_if_gui()
 
         plan_options = {
@@ -290,7 +290,8 @@ def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_PG500_cms, itj_beam_cm, co
 
         if trajectory is None:
             cprint('Client motion planner CANNOT find a plan!', 'red')
-            assert False, 'Client motion planner CANNOT find a plan!'
+            # assert False, 'Client motion planner CANNOT find a plan!'
+            # TODO warning
         else:
             cprint('Client motion planning find a plan!', 'green')
             wait_if_gui('Start sim.')

diff --git a/tests/test_rfl.py b/tests/test_rfl.py
@@ -18,7 +18,7 @@
     joints_from_names, quat_angle_between, get_collision_fn, create_obj, unit_pose, set_camera_pose
 from pybullet_planning import wait_if_gui, wait_for_duration
 from pybullet_planning import plan_cartesian_motion, plan_cartesian_motion_lg
-from pybullet_planning import randomize, elapsed_time
+from pybullet_planning import randomize, elapsed_time, GREY
 
 # from compas_fab.backends import PyBulletClient
 from compas_fab_pychoreo.client import PyChoreoClient
@@ -93,6 +93,7 @@ def test_collision_checker(rfl_setup, itj_TC_PG500_cms, itj_beam_cm, viewer, dia
     move_group = 'robot12_eaYZ'
 
     with PyChoreoClient(viewer=viewer) as client:
+        cprint(urdf_filename, 'green')
         robot = client.load_robot(urdf_filename)
         robot.semantics = semantics
         robot_uid = client.get_robot_pybullet_uid(robot)
@@ -117,14 +118,10 @@ def test_collision_checker(rfl_setup, itj_TC_PG500_cms, itj_beam_cm, viewer, dia
         r11_start_conf_vals = np.array([22700.0, 0.0, -4900.0, 0.0, -80.0, 65.0, 65.0, 20.0, -20.0])
         r12_start_conf_vals = np.array([-4056.0883789999998, -4000.8486330000001, 0.0, -22.834741999999999, -30.711554, 0.0, 57.335655000000003, 0.0])
         full_start_conf = to_rlf_robot_full_conf(r11_start_conf_vals, r12_start_conf_vals)
-        full_joints = joints_from_names(robot_uid, full_start_conf.joint_names)
+        # full_joints = joints_from_names(robot_uid, full_start_conf.joint_names)
 
         client.set_robot_configuration(robot, full_start_conf)
 
-        # # * add static obstacles
-        # for o_cm in itj_rfl_obstacle_cms:
-        #     client.add_collision_mesh(o_cm)
-
         # # * add attachment
         for ee_acm in ee_acms:
             client.add_attached_collision_mesh(ee_acm, options={'robot' : robot, 'mass': 1})