diff --git a/agimus_controller/hpp_panda/scenes.py b/agimus_controller/hpp_panda/scenes.py index 0ba708f6..ee93740e 100644 --- a/agimus_controller/hpp_panda/scenes.py +++ b/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( diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index 68aff29d..3515d3b6 100644 --- a/agimus_controller/ocps/ocp_croco_hpp.py +++ b/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 diff --git a/agimus_controller/robot_model/obstacle_params_parser.py b/agimus_controller/robot_model/obstacle_params_parser.py index 91bb7979..e497882b 100644 --- a/agimus_controller/robot_model/obstacle_params_parser.py +++ b/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: diff --git a/agimus_controller/robot_model/panda_model.py b/agimus_controller/robot_model/panda_model.py index 0a4f430c..333523d1 100644 --- a/agimus_controller/robot_model/panda_model.py +++ b/agimus_controller/robot_model/panda_model.py @@ -46,13 +46,13 @@ 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 @@ -60,4 +60,16 @@ def get_pick_and_place_task_models(): 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) diff --git a/agimus_controller/robot_model/robot_model.py b/agimus_controller/robot_model/robot_model.py index b4055f89..f5542072 100644 --- a/agimus_controller/robot_model/robot_model.py +++ b/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 diff --git a/agimus_controller_ros/agimus_controller.py b/agimus_controller_ros/agimus_controller.py index cf064cbe..50dd424e 100644 --- a/agimus_controller_ros/agimus_controller.py +++ b/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) diff --git a/agimus_controller_ros/controller_base.py b/agimus_controller_ros/controller_base.py index 91a745e6..c1ae715b 100644 --- a/agimus_controller_ros/controller_base.py +++ b/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)