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_ros/agimus_controller.py b/agimus_controller_ros/agimus_controller.py index cf064cbe..1dfa4ec6 100644 --- a/agimus_controller_ros/agimus_controller.py +++ b/agimus_controller_ros/agimus_controller.py @@ -29,11 +29,4 @@ def update_state_machine(self): ): 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..91920dcc 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,9 +182,21 @@ 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 @@ -264,6 +278,15 @@ 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 +312,7 @@ 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 +322,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 +337,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 +370,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 +383,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)