Better support for passive and multiple active groups on kinematic planning

Author: gonzalocasas

CHANGELOG.rst

@@ -17,7 +17,7 @@ Unreleased
 **Changed**
 
 * Updated to COMPAS 0.10
-* Add support for passive joints on IK
+* Add better support for passive joints on IK, Cartesian and Kinematic planning
 
 **Removed**
 

docs/examples/03_backends_ros/files/04_plan_cartesian_motion_ros_loader.py

@@ -17,7 +17,7 @@
     semantics = RobotSemantics.from_srdf_string(srdf, model)
 
     robot = Robot(model, semantics=semantics, client=client)
-    group = 'panda_arm'
+    group = robot.main_group_name
 
     frames = []
     frames.append(Frame((0.2925, 0.3949, 0.5066), (0, 1, 0), (0, 0, 1)))

docs/examples/03_backends_ros/files/04_plan_motion_ros_loader.py

@@ -0,0 +1,42 @@
+import math
+from compas.geometry import Frame
+from compas.robots import RobotModel
+
+from compas_fab.backends import RosClient
+from compas_fab.backends import RosFileServerLoader
+from compas_fab.robots import Configuration
+from compas_fab.robots import Robot
+from compas_fab.robots import RobotSemantics
+
+with RosClient() as client:
+    loader = RosFileServerLoader(client)
+
+    urdf = loader.load_urdf()
+    srdf = loader.load_srdf()
+
+    model = RobotModel.from_urdf_string(urdf)
+    semantics = RobotSemantics.from_srdf_string(srdf, model)
+
+    robot = Robot(model, semantics=semantics, client=client)
+    group = robot.main_group_name
+
+    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
+    tolerance_position = 0.001
+    tolerance_axes = [math.radians(1)] * 3
+
+    start_configuration = Configuration(
+        (0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571), (0, 0, 0, 0, 0, 0, 0))
+
+    # create goal constraints from frame
+    goal_constraints = robot.constraints_from_frame(frame,
+                                                    tolerance_position,
+                                                    tolerance_axes,
+                                                    group)
+
+    trajectory = robot.plan_motion(goal_constraints,
+                                   start_configuration,
+                                   group,
+                                   planner_id='RRT')
+
+    print("Computed kinematic path with %d configurations." % len(trajectory.points))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

src/compas_fab/backends/ros/client.py

@@ -160,23 +160,19 @@ def plan_cartesian_motion(self,
 
         return await_callback(self.plan_cartesian_motion_async, **kwargs)
 
-    def plan_motion(self, goal_constraints, base_link, ee_link, group,
-                    joint_names, joint_types, start_configuration, path_constraints=None,
-                    trajectory_constraints=None, planner_id='',
-                    num_planning_attempts=8, allowed_planning_time=2.,
+    def plan_motion(self, robot, goal_constraints, start_configuration, group,
+                    path_constraints=None, trajectory_constraints=None,
+                    planner_id='', num_planning_attempts=8,
+                    allowed_planning_time=2.,
                     max_velocity_scaling_factor=1.,
                     max_acceleration_scaling_factor=1.,
                     attached_collision_meshes=None,
                     workspace_parameters=None):
-
         kwargs = {}
+        kwargs['robot'] = robot
         kwargs['goal_constraints'] = goal_constraints
-        kwargs['base_link'] = base_link
-        kwargs['ee_link'] = ee_link
-        kwargs['group'] = group
-        kwargs['joint_names'] = joint_names
-        kwargs['joint_types'] = joint_types
         kwargs['start_configuration'] = start_configuration
+        kwargs['group'] = group
         kwargs['path_constraints'] = path_constraints
         kwargs['trajectory_constraints'] = trajectory_constraints
         kwargs['planner_id'] = planner_id

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -215,10 +215,9 @@ def convert_to_trajectory(response):
 
         self.GET_CARTESIAN_PATH(self, request, convert_to_trajectory, errback)
 
-    def plan_motion_async(self, callback, errback, goal_constraints, base_link,
-                          ee_link, group, joint_names, joint_types,
-                          start_configuration, path_constraints=None,
-                          trajectory_constraints=None,
+    def plan_motion_async(self, callback, errback,
+                          robot, goal_constraints, start_configuration, group,
+                          path_constraints=None, trajectory_constraints=None,
                           planner_id='', num_planning_attempts=8,
                           allowed_planning_time=2.,
                           max_velocity_scaling_factor=1.,
@@ -229,6 +228,8 @@ def plan_motion_async(self, callback, errback, goal_constraints, base_link,
 
         # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
         # TODO: if list of frames (goals) => receive multiple solutions?
+        base_link = robot.get_base_link_name(group)
+        joint_names = robot.get_configurable_joint_names(group)
 
         header = Header(frame_id=base_link)
         joint_state = JointState(
@@ -290,10 +291,16 @@ def convert_to_trajectory(response):
             trajectory.source_message = response
             trajectory.fraction = 1.
             trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
-            trajectory.points = convert_trajectory_points(response.trajectory.joint_trajectory.points, joint_types)
-            trajectory.start_configuration = Configuration(response.trajectory_start.joint_state.position, start_configuration.types)
             trajectory.planning_time = response.planning_time
 
+            joint_types = robot.get_joint_types_by_names(trajectory.joint_names)
+            trajectory.points = convert_trajectory_points(
+                response.trajectory.joint_trajectory.points, joint_types)
+
+            start_state = response.trajectory_start.joint_state
+            start_state_types = robot.get_joint_types_by_names(start_state.name)
+            trajectory.start_configuration = Configuration(start_state.position, start_state_types)
+
             callback(trajectory)
 
         self.GET_MOTION_PLAN(self, request, convert_to_trajectory, errback)

src/compas_fab/robots/robot.py

@@ -1217,8 +1217,12 @@ def plan_motion(self, goal_constraints, start_configuration=None,
         if not group:
             group = self.main_group_name  # ensure semantics
 
+        start_configuration = start_configuration.copy() if start_configuration else self.init_configuration()
+
         # Transform goal constraints to RCF and scale
-        T = self._get_current_transformation_WCF_RCF(start_configuration, group)
+        full_configuration = self.merge_group_with_full_configuration(start_configuration, self.init_configuration(), group)
+
+        T = self._get_current_transformation_WCF_RCF(full_configuration, group)
         goal_constraints_RCF_scaled = []
         for c in goal_constraints:
             cp = c.copy()
@@ -1247,30 +1251,22 @@ def plan_motion(self, goal_constraints, start_configuration=None,
         else:
             path_constraints_RCF_scaled = None
 
-        joint_names = self.get_configurable_joint_names()
-        joint_types = self.get_configurable_joint_types(group)
-        start_configuration = start_configuration.copy() if start_configuration else self.init_configuration()
         start_configuration.scale(1. / self.scale_factor)
 
-        kwargs = {}
-        kwargs['goal_constraints'] = goal_constraints_RCF_scaled
-        kwargs['base_link'] = self.get_base_link_name(group)
-        kwargs['ee_link'] = self.get_end_effector_link_name(group)
-        kwargs['group'] = group
-        kwargs['joint_names'] = joint_names
-        kwargs['joint_types'] = joint_types
-        kwargs['start_configuration'] = start_configuration
-        kwargs['path_constraints'] = path_constraints_RCF_scaled
-        kwargs['trajectory_constraints'] = None
-        kwargs['planner_id'] = planner_id
-        kwargs['num_planning_attempts'] = num_planning_attempts
-        kwargs['allowed_planning_time'] = allowed_planning_time
-        kwargs['max_velocity_scaling_factor'] = max_velocity_scaling_factor
-        kwargs['max_acceleration_scaling_factor'] = max_acceleration_scaling_factor
-        kwargs['attached_collision_meshes'] = attached_collision_meshes
-        kwargs['workspace_parameters'] = None
-
-        trajectory = self.client.plan_motion(**kwargs)
+        trajectory = self.client.plan_motion(
+            robot=self,
+            goal_constraints=goal_constraints_RCF_scaled,
+            start_configuration=start_configuration,
+            group=group,
+            path_constraints=path_constraints_RCF_scaled,
+            trajectory_constraints=None,
+            planner_id=planner_id,
+            num_planning_attempts=num_planning_attempts,
+            allowed_planning_time=allowed_planning_time,
+            max_velocity_scaling_factor=max_velocity_scaling_factor,
+            max_acceleration_scaling_factor=max_acceleration_scaling_factor,
+            attached_collision_meshes=attached_collision_meshes,
+            workspace_parameters=None)
 
         # Scale everything back to robot's scale
         for pt in trajectory.points: