Fix some issues with multiple groups in cartesian planner

Author: gonzalocasas

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

@@ -0,0 +1,36 @@
+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 = 'panda_arm'
+
+    frames = []
+    frames.append(Frame((0.2925, 0.3949, 0.5066), (0, 1, 0), (0, 0, 1)))
+    frames.append(Frame((0.5103, 0.2827, 0.4074), (0, 1, 0), (0, 0, 1)))
+
+    start_configuration = Configuration((0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571), (0, 0, 0, 0, 0, 0, 0))
+
+    trajectory = robot.plan_cartesian_motion(frames,
+                                             start_configuration,
+                                             max_step=0.01,
+                                             avoid_collisions=True,
+                                             group=group)
+
+    print("Computed cartesian path with %d configurations, " % len(trajectory.points))
+    print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
+    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

src/compas_fab/backends/ros/client.py

@@ -140,19 +140,16 @@ def forward_kinematics(self, joint_positions, base_link, group, joint_names, ee_
 
         return await_callback(self.forward_kinematics_async, **kwargs)
 
-    def plan_cartesian_motion(self, frames, base_link,
-                              ee_link, group, joint_names, joint_types,
-                              start_configuration, max_step, jump_threshold,
+    def plan_cartesian_motion(self,
+                              robot, frames, start_configuration,
+                              group, max_step, jump_threshold,
                               avoid_collisions, path_constraints,
                               attached_collision_meshes):
         kwargs = {}
+        kwargs['robot'] = robot
         kwargs['frames'] = frames
-        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['max_step'] = max_step
         kwargs['jump_threshold'] = jump_threshold
         kwargs['avoid_collisions'] = avoid_collisions

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -161,18 +161,23 @@ def convert_to_frame(response):
         self.GET_POSITION_FK(self, (header, fk_link_names,
                                     robot_state), convert_to_frame, errback)
 
-    def plan_cartesian_motion_async(self, callback, errback, frames, base_link,
-                                    ee_link, group, joint_names, joint_types,
-                                    start_configuration, max_step, jump_threshold,
+    def plan_cartesian_motion_async(self, callback, errback,
+                                    robot, frames, start_configuration,
+                                    group, max_step, jump_threshold,
                                     avoid_collisions, path_constraints,
                                     attached_collision_meshes):
         """Asynchronous handler of MoveIt cartesian motion planner service."""
+        base_link = robot.get_base_link_name(group)
+        ee_link = robot.get_end_effector_link_name(group)
+
+        joint_names = robot.get_configurable_joint_names(group)
         header = Header(frame_id=base_link)
         waypoints = [Pose.from_frame(frame) for frame in frames]
-        joint_state = JointState(
-            header=header, name=joint_names, position=start_configuration.values)
-        start_state = RobotState(
-            joint_state, MultiDOFJointState(header=header))
+        joint_state = JointState(header=header,
+                                 name=joint_names,
+                                 position=start_configuration.values)
+        start_state = RobotState(joint_state, MultiDOFJointState(header=header))
+
         if attached_collision_meshes:
             for acm in attached_collision_meshes:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
@@ -189,15 +194,24 @@ def plan_cartesian_motion_async(self, callback, errback, frames, base_link,
                        path_constraints=path_constraints)
 
         def convert_to_trajectory(response):
+            try:
             trajectory = JointTrajectory()
             trajectory.source_message = response
             trajectory.fraction = response.fraction
             trajectory.joint_names = response.solution.joint_trajectory.joint_names
+
+                joint_types = robot.get_joint_types_by_names(trajectory.joint_names)
             trajectory.points = convert_trajectory_points(response.solution.joint_trajectory.points, joint_types)
-            trajectory.start_configuration = Configuration(response.start_state.joint_state.position, start_configuration.types)
+
+                start_state_types = robot.get_joint_types_by_names(response.start_state.joint_state.name)
+                trajectory.start_configuration = Configuration(
+                    response.start_state.joint_state.position, start_state_types)
 
             callback(trajectory)
 
+            except Exception as e:
+                errback(e)
+
         self.GET_CARTESIAN_PATH(self, request, convert_to_trajectory, errback)
 
     def plan_motion_async(self, callback, errback, goal_constraints, base_link,

src/compas_fab/robots/robot.py

@@ -1097,14 +1097,9 @@ def plan_cartesian_motion(self, frames_WCF, start_configuration=None,
             frame_RCF = self.to_local_coords(frame_WCF, group)
             frame_RCF.point /= self.scale_factor
             frames_RCF.append(frame_RCF)
-        base_link = self.get_base_link_name(group)
 
-        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)
-
-        ee_link = self.get_end_effector_link_name(group)
         max_step_scaled = max_step / self.scale_factor
 
         T = self.transformation_WCF_RCF(group)
@@ -1123,13 +1118,17 @@ def plan_cartesian_motion(self, frames_WCF, start_configuration=None,
         else:
             path_constraints_RCF_scaled = None
 
-        trajectory = self.client.plan_cartesian_motion(frames_RCF, base_link,
-                                                       ee_link, group, joint_names,
-                                                       joint_types, start_configuration,
-                                                       max_step_scaled, jump_threshold,
-                                                       avoid_collisions,
-                                                       path_constraints_RCF_scaled,
-                                                       attached_collision_meshes)
+        trajectory = self.client.plan_cartesian_motion(
+            robot=self,
+            frames=frames_RCF,
+            start_configuration=start_configuration,
+            group=group,
+            max_step=max_step_scaled,
+            jump_threshold=jump_threshold,
+            avoid_collisions=avoid_collisions,
+            path_constraints=path_constraints_RCF_scaled,
+            attached_collision_meshes=attached_collision_meshes)
+
         # Scale everything back to robot's scale
         for pt in trajectory.points:
             pt.scale(self.scale_factor)