modify panda collision model

agimus_controller/hpp_panda/scenes.py

@@ -150,21 +150,13 @@ def get_shapes_avoiding_collision(self):
         """
         if self._name_scene == "box" or "wall":
             self.shapes_avoiding_collision = [
-                "panda_link7_sc_capsule_0",
-                "panda_link7_sc_capsule_1",
-                "panda_link6_sc_capsule_0",
                 "panda_link5_sc_capsule_1",
-                "panda_link5_sc_capsule_0",
-                "panda_rightfinger_0",
                 "panda_leftfinger_0",
             ]
         elif self._name_scene == "ball":
             self.shapes_avoiding_collision = [
-                "panda_leftfinger_0",
-                "panda_rightfinger_0",
-                "panda_link6_sc_capsule_0",
-                "panda_link5_sc_capsule_0",
                 "panda_link5_sc_capsule_1",
+                "panda_leftfinger_0",
             ]
         else:
             raise NotImplementedError(

agimus_controller/ocps/ocp_croco_hpp.py

@@ -30,7 +30,7 @@ def __init__(
         self._weight_u_reg = self.params.control_weight
         self._weight_ee_placement = self.params.gripper_weight
         self._weight_vel_reg = 0
-        self._collision_margin = 1e-1
+        self._collision_margin = 3e-2
         self.state = crocoddyl.StateMultibody(self._rmodel)
         self.actuation = crocoddyl.ActuationModelFull(self.state)
         self.nq = self._rmodel.nq  # Number of joints of the robot

agimus_controller/robot_model/obstacle_params_parser.py

@@ -192,6 +192,95 @@ def transform_model_into_capsules(
         # Return the copy of the model.
         return model_copy
 
+    def modify_colllision_model(self, rcmodel: pin.GeometryModel):
+        geoms_to_remove_name = [
+            "panda_rightfinger_0",
+            "panda_link7_sc_capsule_1",
+            "panda_link6_sc_capsule_0",
+            "panda_link5_sc_capsule_0",
+            "panda_link4_sc_capsule_0",
+        ]
+
+        for geom_to_remove_name in geoms_to_remove_name:
+            rcmodel.removeGeometryObject(geom_to_remove_name)
+        rcmodel = self.set_panda_ee_capsule_data(rcmodel)
+        rcmodel = self.set_panda_link7_capsule_data(rcmodel)
+        rcmodel = self.set_panda_link5_capsule_data(rcmodel)
+        rcmodel = self.set_panda_link3_capsule_data(rcmodel)
+        return rcmodel
+
+    def set_panda_ee_capsule_data(self, rcmodel: pin.GeometryModel):
+        idx = self.get_geometry_object_idx("panda_leftfinger_0", rcmodel)
+        # copy color of other geometry object
+        if len(rcmodel.geometryObjects) > idx + 1:
+            rcmodel.geometryObjects[idx].meshColor = rcmodel.geometryObjects[
+                idx + 1
+            ].meshColor
+        else:
+            rcmodel.geometryObjects[idx].meshColor = rcmodel.geometryObjects[
+                idx - 1
+            ].meshColor
+        rcmodel.geometryObjects[idx].geometry = Capsule(0.105, 0.048 * 2)
+        rot = np.array(
+            [
+                [0.439545, 0.707107, -0.553896],
+                [-0.439545, 0.707107, 0.553896],
+                [0.783327, 0, 0.62161],
+            ],
+        )
+        rcmodel.geometryObjects[idx].placement.rotation = rot
+        rcmodel.geometryObjects[idx].placement.translation = np.array(
+            [0.032, -0.03, 0.11]
+        )
+        return rcmodel
+
+    def set_panda_link7_capsule_data(self, rcmodel: pin.GeometryModel):
+        idx = self.get_geometry_object_idx("panda_link7_sc_capsule_0", rcmodel)
+        rcmodel.geometryObjects[idx].geometry = Sphere(0.065)
+
+        rcmodel.geometryObjects[idx].placement.translation = np.array(
+            [-0.012, 0.01, -0.025]
+        )
+        return rcmodel
+
+    def set_panda_link5_capsule_data(self, rcmodel: pin.GeometryModel):
+        idx = self.get_geometry_object_idx("panda_link5_sc_capsule_1", rcmodel)
+        rot = np.array(
+            [
+                [0.996802, -0.0799057, -0.00119868],
+                [0.0799147, 0.996689, 0.0149514],
+                [0, -0.0149994, 0.999888],
+            ]
+        )
+        rcmodel.geometryObjects[idx].placement.rotation = rot
+        rcmodel.geometryObjects[idx].placement.translation = np.array([0, 0.03, -0.15])
+        rcmodel.geometryObjects[idx].geometry.radius = 0.095
+        rcmodel.geometryObjects[idx].geometry.halfLength = 0.135
+        return rcmodel
+
+    def set_panda_link3_capsule_data(self, rcmodel: pin.GeometryModel):
+        idx = self.get_geometry_object_idx("panda_link3_sc_capsule_0", rcmodel)
+        rot = np.array(
+            [
+                [0.980067, 0, 0.198669],
+                [0, 1, 0],
+                [-0.198669, 0, 0.980067],
+            ]
+        )
+        rcmodel.geometryObjects[idx].placement.rotation = rot
+        rcmodel.geometryObjects[idx].placement.translation = np.array(
+            [0.035, 0.0, -0.158]
+        )
+        rcmodel.geometryObjects[idx].geometry.radius = 0.13
+        rcmodel.geometryObjects[idx].geometry.halfLength = 0.13
+        return rcmodel
+
+    def get_geometry_object_idx(self, name: str, rcmodel: pin.GeometryModel):
+        for idx, geom in enumerate(rcmodel.geometryObjects):
+            if geom.name == name:
+                return idx
+        return -1
+
     def add_self_collision(
         self, rmodel: pin.Model, rcmodel: pin.GeometryModel, srdf: Path = Path()
     ) -> pin.GeometryModel:

agimus_controller/robot_model/panda_model.py

@@ -46,18 +46,30 @@ def load_model(
             return super().load_model(PandaRobotModelParameters(), env)
 
 
-def get_pick_and_place_task_models():
+def get_task_models(task_name):
     robot_params = PandaRobotModelParameters()
     robot_params.collision_as_capsule = True
     robot_params.self_collision = False
     agimus_demos_description_dir = get_package_path("agimus_demos_description")
     collision_file_path = (
-        agimus_demos_description_dir / "pick_and_place" / "obstacle_params.yaml"
+        agimus_demos_description_dir / task_name / "obstacle_params.yaml"
     )
     robot_constructor = PandaRobotModel.load_model(
         params=robot_params, env=collision_file_path
     )
 
     rmodel = robot_constructor.get_reduced_robot_model()
     cmodel = robot_constructor.get_reduced_collision_model()
-    return rmodel, cmodel
+    vmodel = robot_constructor.get_reduced_visual_model()
+    return rmodel, cmodel, vmodel
+
+
+def get_robot_constructor(task_name):
+    robot_params = PandaRobotModelParameters()
+    robot_params.collision_as_capsule = True
+    robot_params.self_collision = False
+    agimus_demos_description_dir = get_package_path("agimus_demos_description")
+    collision_file_path = (
+        agimus_demos_description_dir / task_name / "obstacle_params.yaml"
+    )
+    return PandaRobotModel.load_model(params=robot_params, env=collision_file_path)

agimus_controller/robot_model/robot_model.py

@@ -57,6 +57,7 @@ def load_model(cls, param: RobotModelParameters, env: Union[Path, None]) -> Self
         model._update_collision_model(
             env, param.collision_as_capsule, param.self_collision, param.srdf
         )
+
         return model
 
     def _load_pinocchio_models(self, urdf: Path, free_flyer: bool) -> None:
@@ -108,6 +109,9 @@ def _update_collision_model(
             self._rcmodel = self._collision_parser.transform_model_into_capsules(
                 self._rcmodel
             )
+            self._rcmodel = self._collision_parser.modify_colllision_model(
+                self._rcmodel
+            )
         if self_collision and srdf.exists():
             self._rcmodel = self._collision_parser.add_self_collision(
                 self._rmodel, self._rcmodel, srdf

agimus_controller_ros/agimus_controller.py

@@ -28,12 +28,3 @@ def update_state_machine(self):
             self.elapsed_time >= 0.1 and self.last_elapsed_time < 0.1
         ):
             self.change_state()
-
-    def change_state(self):
-        self.state_machine_timeline_idx = (self.state_machine_timeline_idx + 1) % len(
-            self.state_machine_timeline
-        )
-        self.state_machine = self.state_machine_timeline[
-            self.state_machine_timeline_idx
-        ]
-        print("changing state to ", self.state_machine)

agimus_controller_ros/controller_base.py

@@ -29,15 +29,16 @@ class HPPStateMachine(Enum):
     WAITING_PICK_TRAJECTORY = 1
     RECEIVING_PICK_TRAJECTORY = 2
     WAITING_PLACE_TRAJECTORY = 3
-    RECEIVING_PLACE_TRAJECTORY = 4
-    WAITING_GOING_INIT_POSE_TRAJECTORY = 5
-    RECEIVING_GOING_INIT_POSE_TRAJECTORY = 6
+    STOP_VISUALISE_SERVOING = 4
+    RECEIVING_PLACE_TRAJECTORY = 5
+    WAITING_GOING_INIT_POSE_TRAJECTORY = 6
+    RECEIVING_GOING_INIT_POSE_TRAJECTORY = 7
 
 
 def find_tracked_object(detections):
     tless1_detections = []
     for detection in detections:
-        if detection.results[0].hypothesis.class_id != "tless-obj_000001":
+        if detection.results[0].hypothesis.class_id != "tless-obj_000023":
             continue
         else:
             tless1_detections.append(detection)
@@ -92,6 +93,7 @@ def initialize_state_machine_attributes(self):
             HPPStateMachine.WAITING_PICK_TRAJECTORY,
             HPPStateMachine.RECEIVING_PICK_TRAJECTORY,
             HPPStateMachine.WAITING_PLACE_TRAJECTORY,
+            HPPStateMachine.STOP_VISUALISE_SERVOING,
             HPPStateMachine.RECEIVING_PLACE_TRAJECTORY,
             HPPStateMachine.WAITING_GOING_INIT_POSE_TRAJECTORY,
             HPPStateMachine.RECEIVING_GOING_INIT_POSE_TRAJECTORY,
@@ -180,10 +182,24 @@ def vision_callback(self, vision_msg: Detection2DArray):
         rot = pose.orientation
         pose_array = [trans.x, trans.y, trans.z, rot.w, rot.x, rot.y, rot.z]
         in_prev_world_M_object = pin.XYZQUATToSE3(pose_array)
-        self.in_world_M_object = self.in_world_M_prev_world * in_prev_world_M_object
+        if self.state_machine in [
+            HPPStateMachine.WAITING_PICK_TRAJECTORY,
+            HPPStateMachine.RECEIVING_PICK_TRAJECTORY,
+            HPPStateMachine.WAITING_PLACE_TRAJECTORY,
+        ]:
+            self.in_world_M_object = self.in_world_M_prev_world * in_prev_world_M_object
         if self.init_in_world_M_object is None:
             self.init_in_world_M_object = self.in_world_M_object
 
+    def change_state(self):
+        self.state_machine_timeline_idx = (self.state_machine_timeline_idx + 1) % len(
+            self.state_machine_timeline
+        )
+        self.state_machine = self.state_machine_timeline[
+            self.state_machine_timeline_idx
+        ]
+        print("changing state to ", self.state_machine)
+
     def wait_first_sensor_msg(self):
         wait_for_input = True
         while not rospy.is_shutdown() and wait_for_input:
@@ -264,6 +280,19 @@ def solve(self, x0):
         ):
             self.update_effector_placement_with_vision()
             self.set_increasing_weight()
+            current_pose = get_ee_pose_from_configuration(
+                self.rmodel,
+                self.rdata,
+                self.effector_frame_id,
+                x0[: self.rmodel.nq],
+            ).translation
+            if (
+                np.linalg.norm(self.in_world_M_effector.translation - current_pose)
+                < 0.01
+                and self.state_machine == HPPStateMachine.WAITING_PLACE_TRAJECTORY
+            ):
+                print("changing state for visual servoing")
+                self.change_state()
         else:
             self.mpc.ocp.set_last_running_model_placement_weight(0)
         self.mpc.mpc_step(x0, new_x_ref, new_a_ref, self.in_world_M_effector)
@@ -289,9 +318,6 @@ def set_increasing_weight(self):
         self.mpc.ocp.set_ee_placement_weight(gripper_weight_terminal_node)
 
     def update_effector_placement_with_vision(self):
-        in_object_rot_effector = (
-            self.init_in_world_M_object.rotation.T * self.in_world_M_effector.rotation
-        )
         self.target_translation_object_to_effector = (
             self.in_world_M_effector.translation
             - self.init_in_world_M_object.translation
@@ -301,9 +327,13 @@ def update_effector_placement_with_vision(self):
             self.in_world_M_object.translation
             + self.target_translation_object_to_effector
         )
-        self.in_world_M_effector.rotation = (
-            self.in_world_M_object.rotation * in_object_rot_effector
-        )
+        # Currently not used because it needs the implementation of symmetries
+        # in_object_rot_effector = (
+        #    self.init_in_world_M_object.rotation.T * self.in_world_M_effector.rotation
+        # )
+        # self.in_world_M_effector.rotation = (
+        #    self.in_world_M_object.rotation * in_object_rot_effector
+        # )
 
     def pick_traj_last_point_is_in_horizon(self):
         return (
@@ -312,6 +342,8 @@ def pick_traj_last_point_is_in_horizon(self):
         )
 
     def pick_traj_last_point_is_near(self, x0):
+        if self.state_machine == HPPStateMachine.STOP_VISUALISE_SERVOING:
+            return True
         if (
             self.state_machine == HPPStateMachine.WAITING_PLACE_TRAJECTORY
             and self.pick_traj_last_pose is None
@@ -343,8 +375,10 @@ def pick_traj_last_point_is_near(self, x0):
                         and np.linalg.norm(self.pick_traj_last_pose - current_pose)
                         < self.params.start_visual_servoing_dist
                     )
+
                     if self.do_visual_servoing:
                         self.start_visual_servoing_time = time.time()
+
             else:
                 self.do_visual_servoing = False
             return self.do_visual_servoing
@@ -354,7 +388,15 @@ def get_sensor_msg(self):
             sensor_msg = deepcopy(self.sensor_msg)
         return sensor_msg
 
+    def set_control_inside_limits(self, u):
+        control_limits = [86] * 4 + [11] * 3
+        for idx, control_limit in enumerate(control_limits):
+            if np.abs(u[idx]) > control_limit:
+                u[idx] *= control_limit / np.abs(u[idx])
+        return u
+
     def send(self, sensor_msg, u, k):
+        u = self.set_control_inside_limits(u)
         self.control_msg.header = Header()
         self.control_msg.header.stamp = rospy.Time.now()
         self.control_msg.feedback_gain = to_multiarray_f64(k)