Add support for passive joints on IK

Author: gonzalocasas

CHANGELOG.rst

@@ -17,6 +17,7 @@ Unreleased
 **Changed**
 
 * Updated to COMPAS 0.10
+* Add support for passive joints on IK
 
 **Removed**
 

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -141,7 +141,7 @@ def inverse_kinematics_async(self, callback, errback, frame, base_link, group,
                                        attempts=attempts)
 
         def convert_to_positions(response):
-            callback(response.solution.joint_state.position)
+            callback((response.solution.joint_state.position, response.solution.joint_state.name))
 
         self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions, errback)
 

src/compas_fab/robots/robot.py

@@ -514,16 +514,12 @@ def get_position_by_joint_name(self, configuration, joint_name, group=None):
                 "Please pass a configuration with %d values or specify group" % len(names))
         return configuration.values[names.index(joint_name)]
 
-    def _scale_joint_values(self, values, scale_factor, group=None):
+    def _scale_joint_values(self, values, names, scale_factor, group=None):
         """Scales the scaleable joint values with scale_factor.
         """
-        joints = self.get_configurable_joints(group)
-        if len(joints) != len(values):
-            raise ValueError("Expected %d values for group %s, but received only %d." % (
-                len(joints), group, len(values)))
-
         values_scaled = []
-        for v, j in zip(values, joints):
+        for v, name in zip(values, names):
+            j = self.get_joint_by_name(name)
             if j.is_scalable():
                 v *= scale_factor
             values_scaled.append(v)
@@ -540,7 +536,7 @@ def _get_scaled_joint_positions_from_start_configuration(self, start_configurati
             joint_positions = start_configuration.values
         # scale the prismatic joints
         joint_positions = self._scale_joint_values(
-            joint_positions, 1. / self.scale_factor)
+            joint_positions, joint_names, 1. / self.scale_factor)
         return joint_positions
 
     # ==========================================================================
@@ -906,14 +902,22 @@ def inverse_kinematics(self, frame_WCF, start_configuration=None,
         frame_RCF = self.to_local_coords(frame_WCF, group)
         frame_RCF.point /= self.scale_factor  # must be in meters
 
-        joint_positions = self.client.inverse_kinematics(frame_RCF, base_link,
-                                                         group, joint_names, joint_positions,
-                                                         avoid_collisions, constraints, attempts,
-                                                         attached_collision_meshes)
-        joint_positions = self._scale_joint_values(joint_positions, self.scale_factor)
-        # full configuration # TODO group config?
-        configuration = Configuration(joint_positions, self.get_configurable_joint_types())
+        # The returned joint names might be more than the requested ones if there are passive joints present
+        joint_positions, joint_names = self.client.inverse_kinematics(frame_RCF, base_link,
+                                                                      group, joint_names, joint_positions,
+                                                                      avoid_collisions, constraints, attempts,
+                                                                      attached_collision_meshes)
+        joint_types = [self.get_joint_by_name(n).type for n in joint_names]
+
+        joint_positions = self._scale_joint_values(joint_positions, joint_names, self.scale_factor)
+
+        # build configuration including passive joints
+        configuration = Configuration(joint_positions, joint_types)
 
+        # return only group configuration
+        # NOTE: it might actually make more sense to return
+        # the configuration instance without extracting the group's config
+        # because we lose the passive joint info
         return self.get_group_configuration(group, configuration)
 
     def forward_kinematics(self, configuration, group=None, backend=None, link_name=None):