pybullet set_robot_configuration support mimic joint

src/compas_fab/backends/pybullet/client.py

@@ -669,12 +669,29 @@ def set_robot_configuration(self, configuration):
             configuration.joint_names != []
         ), "Joint names must be provided in the configuration passed to set_robot_configuration."
 
-        for joint_name, joint_value in zip(configuration.joint_names, configuration.joint_values):
-            assert joint_name, "Joint name must be provided in the configuration."
-            self._set_joint_position(self.robot_joint_puids[joint_name], joint_value, self.robot_puid)
+        # Iterate through all joints that are considered free by PyBullet
+        for joint_name, joint_puid in self.get_pose_joint_names_and_puids():
+            if joint_name in configuration:
+                self._set_joint_position(joint_puid, configuration[joint_name], self.robot_puid)
+            else:
+                # Check if this is mimic joint
+                joint = self.robot.model.get_joint_by_name(joint_name)
+                mimic = joint.mimic  # type: Mimic
+                # Get the value of the joint that is being mimicked (works only for non-cascaded mimic)
+                if mimic:
+                    mimicked_joint_position = configuration[mimic.joint]
+                    self._set_joint_position(
+                        joint_puid, mimic.calculate_position(mimicked_joint_position), self.robot_puid
+                    )
+                else:
+                    raise ValueError(
+                        "Joint value for '{}' is needed for Pybullet but not found in the provided configuration.".format(
+                            joint_name
+                        )
+                    )
 
-    def get_robot_configuration(self, robot):
-        # type: (Robot) -> Configuration
+    def get_robot_configuration(self):
+        # type: () -> Configuration
         """Gets the robot's current pose.
 
         Parameters
@@ -685,11 +702,12 @@ def get_robot_configuration(self, robot):
         -------
         :class:`compas_robots.Configuration`
         """
+        robot = self.robot_cell.robot
         configuration = robot.zero_configuration()
-
-        joint_ids_ordered = [self.robot_joint_puids[joint_name] for joint_name in configuration.joint_names]
-        joint_values_ordered = self._get_joint_positions(joint_ids_ordered, self.robot_puid)
-        configuration.joint_values = joint_values_ordered
+        for joint_name in configuration.joint_names:
+            joint_id = self.robot_joint_puids[joint_name]
+            joint_value = self._get_joint_positions([joint_id], self.robot_puid)[0]
+            configuration[joint_name] = joint_value
 
         return configuration
 

tests/backends/pybullet/test_pybullet_client.py

@@ -4,6 +4,7 @@
 from compas_fab.backends import PyBulletClient
 from compas_robots import RobotModel
 from compas_fab.robots import Robot
+from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotLibrary
 
 from compas_robots.resources import LocalPackageMeshLoader
@@ -92,6 +93,28 @@ def set_and_check_robot(robot):
         set_and_check_robot(RobotLibrary.rfl())
 
 
+def test_pybullet_client_set_robot_configuration():
+    with PyBulletClient(connection_type="direct") as client:
+        robot = RobotLibrary.panda(load_geometry=True)
+        # Typically user would call planner.set_robot_cell() directly
+        robot = client.set_robot(robot)
+        client._robot_cell = RobotCell(robot)
+
+        # Set configuration
+        configuration = robot.model.zero_configuration()
+        client.set_robot_configuration(configuration)
+        # Check that the configuration is set
+        assert client.get_robot_configuration().close_to(configuration)
+
+        # Try to set the finger position
+        configuration["panda_finger_joint1"] = 0.02
+        client.set_robot_configuration(configuration)
+        # Check that the joint and the mimic joint are set
+        joint_ids = [client.robot_joint_puids["panda_finger_joint1"], client.robot_joint_puids["panda_finger_joint2"]]
+        joint_values = client._get_joint_positions(joint_ids, client.robot_puid)
+        assert joint_values == [0.02, 0.02]
+
+
 def test_pybullet_client_internal_puids():
     with PyBulletClient(connection_type="direct") as client:
         assert len(client.robot_link_puids) == 0

tests/backends/pybullet/test_pybullet_planner.py

@@ -234,6 +234,7 @@ def test_pybullet_ik_fk_agreement_abb_irb4600_40_255():
 
 
 def test_pybullet_ik_fk_agreement_panda():
+    # The panda robot has mimic joints for testing purposes
     robot = RobotLibrary.panda(load_geometry=True)
 
     ik_center_frame = Frame(