From 3f26fd1ea5c06b7a6fa8cf9e2a22989d618435be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Fri, 22 Nov 2024 12:55:37 +0000 Subject: [PATCH 01/10] modify panda collision model --- agimus_controller/hpp_panda/scenes.py | 10 +-- .../robot_model/obstacle_params_parser.py | 89 +++++++++++++++++++ agimus_controller/robot_model/panda_model.py | 18 +++- agimus_controller/robot_model/robot_model.py | 4 + 4 files changed, 109 insertions(+), 12 deletions(-) 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/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 From 397990375a403e9cf282d15c2ab0258d4381ac86 Mon Sep 17 00:00:00 2001 From: kzorina Date: Tue, 26 Nov 2024 15:49:11 +0000 Subject: [PATCH 02/10] changing state machine, fix collision safety distance --- agimus_controller/ocps/ocp_croco_hpp.py | 2 +- agimus_controller_ros/agimus_controller.py | 9 +--- agimus_controller_ros/controller_base.py | 59 ++++++++++++++++++---- 3 files changed, 50 insertions(+), 20 deletions(-) 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) From 61cfdfd713f09aee43b1ed2dc44e9f52951ca118 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 26 Nov 2024 16:00:39 +0000 Subject: [PATCH 03/10] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- agimus_controller_ros/agimus_controller.py | 2 -- agimus_controller_ros/controller_base.py | 41 ++++++++++++---------- 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/agimus_controller_ros/agimus_controller.py b/agimus_controller_ros/agimus_controller.py index 1dfa4ec6..50dd424e 100644 --- a/agimus_controller_ros/agimus_controller.py +++ b/agimus_controller_ros/agimus_controller.py @@ -28,5 +28,3 @@ def update_state_machine(self): self.elapsed_time >= 0.1 and self.last_elapsed_time < 0.1 ): self.change_state() - - diff --git a/agimus_controller_ros/controller_base.py b/agimus_controller_ros/controller_base.py index 91920dcc..c1ae715b 100644 --- a/agimus_controller_ros/controller_base.py +++ b/agimus_controller_ros/controller_base.py @@ -182,13 +182,15 @@ 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) - if self.state_machine in [HPPStateMachine.WAITING_PICK_TRAJECTORY, + if self.state_machine in [ + HPPStateMachine.WAITING_PICK_TRAJECTORY, HPPStateMachine.RECEIVING_PICK_TRAJECTORY, - HPPStateMachine.WAITING_PLACE_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 @@ -284,9 +286,13 @@ def solve(self, x0): 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() + 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) @@ -312,7 +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): - self.target_translation_object_to_effector = ( self.in_world_M_effector.translation - self.init_in_world_M_object.translation @@ -323,12 +328,12 @@ def update_effector_placement_with_vision(self): + self.target_translation_object_to_effector ) # Currently not used because it needs the implementation of symmetries - #in_object_rot_effector = ( + # 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_effector.rotation = ( # self.in_world_M_object.rotation * in_object_rot_effector - #) + # ) def pick_traj_last_point_is_in_horizon(self): return ( @@ -370,10 +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 @@ -383,11 +388,11 @@ 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]) + 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): From f8897c5d7810024f2b0a8ab31aa954b4a3a78e61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 27 Nov 2024 12:37:10 +0000 Subject: [PATCH 04/10] add functions to use state reg final node --- agimus_controller/ocps/ocp_croco_hpp.py | 103 +++++++++++++++++++++--- 1 file changed, 92 insertions(+), 11 deletions(-) diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index 68aff29d..01c10f38 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 @@ -161,6 +161,34 @@ def set_running_models(self): self.running_models = running_models return self.running_models + def get_model(self, placement_ref, x_ref: np.ndarray, u_ref: np.ndarray): + running_cost_model = crocoddyl.CostModelSum(self.state) + x_reg_cost = self.get_state_residual(x_ref) + u_reg_cost = self.get_control_residual(u_ref) + vel_reg_cost = self.get_velocity_residual() + placement_ref = get_ee_pose_from_configuration( + self._rmodel, self._rdata, self._effector_frame_id, x_ref[: self.nq] + ) + placement_reg_cost = self.get_placement_residual(placement_ref) + running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) + running_cost_model.addCost("uReg", u_reg_cost, self._weight_u_reg) + running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg) + running_cost_model.addCost( + "gripperPose", placement_reg_cost, 0 + ) # useful for mpc to reset ocp + + if self.params.use_constraints: + constraints = self.get_constraints() + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, running_cost_model, constraints + ) + else: + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, running_cost_model + ) + running_DAM.armature = self.params.armature + return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt) + def get_constraints(self): constraint_model_manager = crocoddyl.ConstraintModelManager(self.state, self.nq) if len(self._cmodel.collisionPairs) != 0: @@ -176,6 +204,10 @@ def get_constraints(self): def set_terminal_model(self, placement_ref): """Set terminal model.""" + # last_model = self.get_new_weight_terminal_model_without_constraints( + # placement_ref, self.x_plan[-1, :], self.u_plan[-1, :] + # ) + if self.params.use_constraints: last_model = self.get_terminal_model_with_constraints( placement_ref, self.x_plan[-1, :], self.u_plan[-1, :] @@ -239,6 +271,35 @@ def get_terminal_model_with_constraints( terminal_DAM.armature = self.params.armature return crocoddyl.IntegratedActionModelEuler(terminal_DAM, self.params.dt) + def get_new_weight_terminal_model_without_constraints( + self, placement_ref, x_ref: np.ndarray, u_plan: np.ndarray + ): + """Return last model without constraints.""" + + terminal_cost_model = crocoddyl.CostModelSum(self.state) + placement_reg_cost = self.get_placement_residual(placement_ref) + terminal_cost_model.addCost("gripperPose", placement_reg_cost, 0) + vel_cost = self.get_velocity_residual() + if np.linalg.norm(x_ref[self.nq :]) < 1e-9: + terminal_cost_model.addCost("velReg", vel_cost, 0) + else: + terminal_cost_model.addCost("velReg", vel_cost, 0) + stateWeightsTerm = np.array([1.0] * self.nq + [0.1] * self.nv) ** 2 + x_reg_cost = self.get_state_residual(x_ref, x_reg_weights=stateWeightsTerm) + + terminal_cost_model.addCost( + "xReg", x_reg_cost, self._weight_x_reg * self.params.dt * 1000 + ) + + u_reg_cost = self.get_control_residual(u_plan) + terminal_cost_model.addCost("uReg", u_reg_cost, 0) + constraints = self.get_constraints() + terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, terminal_cost_model, constraints + ) + terminal_DAM.armature = self.params.armature + return crocoddyl.IntegratedActionModelEuler(terminal_DAM, self.params.dt) + def _get_collision_constraint( self, col_idx: int, safety_margin: float ) -> "crocoddyl.ConstraintModelResidual": @@ -292,12 +353,19 @@ def get_control_residual(self, uref): self.state, crocoddyl.ResidualModelControl(self.state, uref) ) - def get_state_residual(self, xref): + def get_state_residual(self, xref, x_reg_weights=None): """Return state residual with xref the state reference.""" - return crocoddyl.CostModelResidual( - self.state, # x_reg_weights, - crocoddyl.ResidualModelState(self.state, xref, self.actuation.nu), - ) + if x_reg_weights is None: + return crocoddyl.CostModelResidual( + self.state, + crocoddyl.ResidualModelState(self.state, xref, self.actuation.nu), + ) + else: + return crocoddyl.CostModelResidual( + self.state, + crocoddyl.ActivationModelWeightedQuad(x_reg_weights), + crocoddyl.ResidualModelState(self.state, xref, self.actuation.nu), + ) def get_xlimit_residual(self): """Return state limit residual.""" @@ -331,11 +399,9 @@ def get_inverse_dynamic_control(self, x, a): def update_cost(self, model, new_model, cost_name, update_weight=True): """Update model's cost reference and weight by copying new_model's cost.""" - model.differential.costs.costs[ - cost_name - ].cost.residual.reference = new_model.differential.costs.costs[ - cost_name - ].cost.residual.reference.copy() + model.differential.costs.costs[cost_name].cost.residual.reference = ( + new_model.differential.costs.costs[cost_name].cost.residual.reference.copy() + ) if update_weight: new_weight = new_model.differential.costs.costs[cost_name].weight model.differential.costs.costs[cost_name].weight = new_weight @@ -347,6 +413,21 @@ def update_model(self, model, new_model, update_weight): self.update_cost(model, new_model, "velReg", update_weight) self.update_cost(model, new_model, "uReg", update_weight) + # def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): + # """Reset ocp problem using next reference in state and control.""" + # self.solver.problem.x0 = x + # runningModels = list(self.solver.problem.runningModels) + # for node_idx in range(len(runningModels) - 1): + # self.update_model( + # runningModels[node_idx], runningModels[node_idx + 1], True + # ) + # self.update_model(runningModels[-1], self.solver.problem.terminalModel, False) + # + # terminal_model = self.get_new_weight_terminal_model_without_constraints( + # placement_ref, x_ref, u_plan + # ) + # self.update_model(self.solver.problem.terminalModel, terminal_model, True) + def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): """Reset ocp problem using next reference in state and control.""" self.solver.problem.x0 = x From 763888dfc45649f68a2204edcedbaf1490b0518e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 27 Nov 2024 14:55:47 +0000 Subject: [PATCH 05/10] working --- README.md | 2 +- agimus_controller/hpp_panda/scenes.py | 4 +- agimus_controller/main/panda/main_hpp_mpc.py | 23 ++++++-- agimus_controller/mpc.py | 29 ++++++---- agimus_controller/mpc_search.py | 57 ++++++++++++------- .../robot_model/obstacle_params_parser.py | 3 +- agimus_controller/robot_model/robot_model.py | 6 +- agimus_controller/utils/ocp_analyzer.py | 16 +++--- .../reaching_goal_controller.py | 4 +- 9 files changed, 90 insertions(+), 54 deletions(-) diff --git a/README.md b/README.md index 72578a64..ea7a4f7e 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ Different mains are available: - panda scripts : - `python3 -m agimus_controller.main.panda.main_hpp_mpc_buffer` - `python3 -m agimus_controller.main.panda.main_hpp_mpc` - - `python3 -m agimus_controller.main.panda.main_reaching_goal` + - `python3 -im agimus_controller.main.panda.main_reaching_goal` - `python3 -m agimus_controller.main.panda.main_meshcat_display` - `python3 -m agimus_controller.main.panda.main_optim_traj` - `python3 -m agimus_controller.main.panda.main_scenes` diff --git a/agimus_controller/hpp_panda/scenes.py b/agimus_controller/hpp_panda/scenes.py index ee93740e..0cfd4ec8 100644 --- a/agimus_controller/hpp_panda/scenes.py +++ b/agimus_controller/hpp_panda/scenes.py @@ -150,12 +150,12 @@ def get_shapes_avoiding_collision(self): """ if self._name_scene == "box" or "wall": self.shapes_avoiding_collision = [ - "panda_link5_sc_capsule_1", + "panda_link5_sc_capsule_0", "panda_leftfinger_0", ] elif self._name_scene == "ball": self.shapes_avoiding_collision = [ - "panda_link5_sc_capsule_1", + "panda_link5_sc_capsule_0", "panda_leftfinger_0", ] else: diff --git a/agimus_controller/main/panda/main_hpp_mpc.py b/agimus_controller/main/panda/main_hpp_mpc.py index a7edb4f1..c4efb97f 100644 --- a/agimus_controller/main/panda/main_hpp_mpc.py +++ b/agimus_controller/main/panda/main_hpp_mpc.py @@ -11,6 +11,13 @@ from agimus_controller.ocps.parameters import OCPParameters from agimus_controller.main.servers import Servers +from agimus_controller.utils.ocp_analyzer import ( + return_cost_vectors, + return_constraint_vector, + plot_costs_from_dic, + plot_constraints_from_dic, +) + class APP(object): def main(self, use_gui=False, spawn_servers=False): @@ -43,22 +50,26 @@ def main(self, use_gui=False, spawn_servers=False): ocp = OCPCrocoHPP(rmodel, cmodel, ocp_params) - mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel) + self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel) start = time.time() - mpc.simulate_mpc(save_predictions=False) + self.mpc.simulate_mpc(save_predictions=False) end = time.time() print("Time of solving: ", end - start) - u_plan = mpc.ocp.get_u_plan(x_plan, a_plan) + costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True) + constraint = return_constraint_vector(self.mpc.ocp.solver) + plot_costs_from_dic(costs) + plot_constraints_from_dic(constraint) + u_plan = self.mpc.ocp.get_u_plan(x_plan, a_plan) self.mpc_plots = MPCPlots( - croco_xs=mpc.croco_xs, - croco_us=mpc.croco_us, + croco_xs=self.mpc.croco_xs, + croco_us=self.mpc.croco_us, whole_x_plan=x_plan, whole_u_plan=u_plan, rmodel=rmodel, vmodel=pandawrapper.get_reduced_visual_model(), cmodel=cmodel, - DT=mpc.ocp.params.dt, + DT=self.mpc.ocp.params.dt, ee_frame_name=ee_frame_name, viewer=viewer, ) diff --git a/agimus_controller/mpc.py b/agimus_controller/mpc.py index 3dc58934..961e40d5 100644 --- a/agimus_controller/mpc.py +++ b/agimus_controller/mpc.py @@ -1,5 +1,6 @@ from __future__ import annotations import numpy as np +import time from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration @@ -74,12 +75,14 @@ def simulate_mpc(self, save_predictions=False): x_plan = self.whole_x_plan[:T, :] a_plan = self.whole_a_plan[:T, :] self.ocp.set_planning_variables(x_plan, a_plan) + start_mpc_step_time = time.time() x, u0 = self.mpc_first_step(x_plan, self.ocp.u_plan[: T - 1], x0) + mpc_step_time = time.time() - start_mpc_step_time mpc_xs[1, :] = x mpc_us[0, :] = u0 next_node_idx = T if save_predictions: - self.create_mpc_data() + self.create_mpc_data(step_time=mpc_step_time) for idx in range(1, self.whole_traj_T - 1): x_plan = self.update_planning(x_plan, self.whole_x_plan[next_node_idx, :]) a_plan = self.update_planning(a_plan, self.whole_a_plan[next_node_idx, :]) @@ -89,13 +92,15 @@ def simulate_mpc(self, save_predictions=False): self.ocp._effector_frame_id, x_plan[-1, : self.nq], ) + start_mpc_step_time = time.time() x, u = self.mpc_step(x, x_plan[-1], a_plan[-1], placement_ref) + mpc_step_time = time.time() - start_mpc_step_time if next_node_idx < self.whole_x_plan.shape[0] - 1: next_node_idx += 1 mpc_xs[idx + 1, :] = x mpc_us[idx, :] = u if save_predictions: - self.fill_predictions_and_refs_arrays() + self.fill_predictions_and_refs_arrays(step_time=mpc_step_time) self.croco_xs = mpc_xs self.croco_us = mpc_us if save_predictions: @@ -135,7 +140,7 @@ def mpc_step(self, x0, new_x_ref, new_a_ref, placement_ref): x0 = self.get_next_state(x0, self.ocp.solver.problem) return x0, self.ocp.solver.us[0] - def create_mpc_data(self): + def create_mpc_data(self, step_time=None): xs, us = self.get_predictions() x_ref, p_ref, u_ref = self.get_reference() self.mpc_data["preds_xs"] = [xs] @@ -148,8 +153,10 @@ def create_mpc_data(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() self.mpc_data["coll_residuals"] = collision_residuals + if step_time is not None: + self.mpc_data["step_time"] = [step_time] - def fill_predictions_and_refs_arrays(self): + def fill_predictions_and_refs_arrays(self, step_time=None): xs, us = self.get_predictions() x_ref, p_ref, u_ref = self.get_reference() self.mpc_data["preds_xs"].append(xs) @@ -162,9 +169,11 @@ def fill_predictions_and_refs_arrays(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][coll_residual_key] += ( - collision_residuals[coll_residual_key] - ) + self.mpc_data["coll_residuals"][ + coll_residual_key + ] += collision_residuals[coll_residual_key] + if step_time is not None: + self.mpc_data["step_time"].append(step_time) def get_ee_pred_from_xs_pred(self, xs): pred_ee = np.zeros((xs.shape[0], 3)) @@ -192,7 +201,7 @@ def save_ocp_data(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][coll_residual_key] += ( - collision_residuals[coll_residual_key] - ) + self.mpc_data["coll_residuals"][ + coll_residual_key + ] += collision_residuals[coll_residual_key] np.save("ocp_data.npy", ocp_data) diff --git a/agimus_controller/mpc_search.py b/agimus_controller/mpc_search.py index 682608de..caa25ec0 100644 --- a/agimus_controller/mpc_search.py +++ b/agimus_controller/mpc_search.py @@ -22,6 +22,7 @@ def __init__(self, mpc: MPC, rmodel, ee_frame_name): self.results["max_us"] = [] self.results["max_increase_us"] = [] self.results["combination"] = [] + self.results["max_kkt"] = [] def get_trajectory_difference(self, configuration_traj=True): """Compute at each node the absolute difference in position either in cartesian or configuration space and sum it.""" @@ -69,27 +70,36 @@ def search_best_costs(self, use_constraints=False, configuration_traj=False): self.best_combination = None self.best_croco_xs = None self.best_croco_us = None - self.best_diff = 1e6 + self.best_kkt = 1e6 grip_cost = 0 self.mpc.ocp.use_constraints = use_constraints if use_constraints: - for x_exponent in range(0, 8, 2): - for u_exponent in range(-32, -26, 2): - start = time.time() - _, x_cost, u_cost, _, _ = self.get_cost_from_exponent( - 0, x_exponent, u_exponent, 0, 0 - ) - print(" x : ", x_cost, " u : ", u_cost) - self.try_new_costs( - grip_cost, - x_cost, - u_cost, - configuration_traj=configuration_traj, - vel_cost=0, - xlim_cost=0, - ) - end = time.time() - print("mpc simulation duration ", end - start) + for grip_exponent in range(0, 6, 2): + for vel_exponent in range(-5, 0, 5): + for u_exponent in range(-34, -28, 2): + start = time.time() + grip_cost, _, u_cost, vel_cost, _ = self.get_cost_from_exponent( + grip_exponent, 0, u_exponent, vel_exponent, 0 + ) + self.mpc.ocp.set_weights(grip_cost, 0, u_cost, vel_cost) + print( + " grip : ", + grip_cost, + " u : ", + u_cost, + " vel cost", + vel_cost, + ) + self.try_new_costs( + grip_cost, + x_cost=0, + u_cost=u_cost, + configuration_traj=configuration_traj, + vel_cost=vel_cost, + xlim_cost=0, + ) + end = time.time() + print("mpc simulation duration ", end - start) else: for grip_exponent in range(25, 50, 5): for x_exponent in range(0, 15, 5): @@ -120,7 +130,7 @@ def search_best_costs(self, use_constraints=False, configuration_traj=False): self.croco_xs = self.best_croco_xs self.croco_us = self.best_croco_us - print("best diff ", self.best_diff) + print("best kkt ", self.best_kkt) print("best combination ", self.best_combination) print("max torque ", np.max(np.abs(self.croco_us))) @@ -145,7 +155,7 @@ def try_new_costs( configuration_traj=False, ): """Set problem, run solver, add result in dict and check if we found a better solution.""" - self.mpc.simulate_mpc(100) + self.mpc.simulate_mpc(save_predictions=True) self.croco_xs = self.mpc.croco_xs self.croco_us = self.mpc.croco_us max_us = np.max(np.abs(self.croco_us)) @@ -157,10 +167,13 @@ def try_new_costs( self.results["combination"].append( [grip_cost, x_cost, u_cost, vel_cost, xlim_cost] ) + max_kkt = max(self.mpc.mpc_data["kkt_norm"]) + print("max kkt ", max_kkt) + self.results["max_kkt"].append(max_kkt) diff = self.get_trajectory_difference(configuration_traj) - if diff < self.best_diff and max_us < 100 and max_increase_us < 50: + if max_kkt < self.best_kkt and max_us < 100 and max_increase_us < 50: self.best_combination = [grip_cost, x_cost, u_cost, vel_cost, xlim_cost] - self.best_diff = diff + self.best_kkt = max_kkt self.best_croco_xs = self.croco_xs self.best_croco_us = self.croco_us diff --git a/agimus_controller/robot_model/obstacle_params_parser.py b/agimus_controller/robot_model/obstacle_params_parser.py index e497882b..05ba4218 100644 --- a/agimus_controller/robot_model/obstacle_params_parser.py +++ b/agimus_controller/robot_model/obstacle_params_parser.py @@ -168,7 +168,7 @@ def transform_model_into_capsules( capsule_name, parentFrame, parentJoint, - Capsule(geometry.radius, geometry.halfLength), + Capsule(geometry.radius, geometry.halfLength * 2), placement, ) RED = np.array([249, 136, 126, 125]) / 255 @@ -195,7 +195,6 @@ def transform_model_into_capsules( 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", diff --git a/agimus_controller/robot_model/robot_model.py b/agimus_controller/robot_model/robot_model.py index f5542072..4fe84d33 100644 --- a/agimus_controller/robot_model/robot_model.py +++ b/agimus_controller/robot_model/robot_model.py @@ -109,9 +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 - ) + # 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/utils/ocp_analyzer.py b/agimus_controller/utils/ocp_analyzer.py index 153aa53a..8f38fb94 100644 --- a/agimus_controller/utils/ocp_analyzer.py +++ b/agimus_controller/utils/ocp_analyzer.py @@ -88,9 +88,11 @@ def return_cost_vectors(ddp, weighted=False, integrated=False): cost_tag ].cost if weighted: - costs[cost_tag][-1] *= ( - ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight - ) + costs[cost_tag][ + -1 + ] *= ddp.problem.terminalModel.differential.costs.costs.todict()[ + cost_tag + ].weight except Exception as ex: print(ex.with_traceback()) costs[cost_tag][-1] = np.mean( @@ -126,7 +128,7 @@ def return_constraint_vector(solver): else: num_components = 1 constraints[constraint_tag] = np.nan * np.ones( - (solver.problem.T + 1, num_components) + (solver.problem.T, num_components) ) try: if num_components == 1: @@ -158,9 +160,9 @@ def return_weights(ddp): ): if cost_tag not in weights: weights.update({cost_tag: np.nan * np.ones(ddp.problem.T + 1)}) - weights[cost_tag][-1] = ( - ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight - ) + weights[cost_tag][ + -1 + ] = ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight return weights diff --git a/agimus_controller_ros/reaching_goal_controller.py b/agimus_controller_ros/reaching_goal_controller.py index da3d398f..fa27000f 100644 --- a/agimus_controller_ros/reaching_goal_controller.py +++ b/agimus_controller_ros/reaching_goal_controller.py @@ -21,7 +21,9 @@ class ReachingGoalController: def __init__(self, params: AgimusControllerNodeParameters) -> None: self.params = params - self.rmodel, self.cmodel = get_task_models(task_name="goal_reaching") + self.rmodel, self.cmodel, self.vmodel = get_task_models( + task_name="reaching_goal" + ) self.rdata = self.rmodel.createData() self.effector_frame_id = self.rmodel.getFrameId( self.params.ocp.effector_frame_name From 7b7e9194d83fef30fa2a6de09623d3c6308e6b73 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Thu, 16 Jan 2025 08:21:07 +0000 Subject: [PATCH 06/10] slight changes in ocp and stored mpc data --- agimus_controller/main/panda/main_hpp_mpc.py | 2 +- .../main/panda/main_reaching_goal.py | 9 ++- agimus_controller/mpc.py | 5 +- agimus_controller/ocps/ocp_croco_hpp.py | 61 ++++++----------- agimus_controller/ocps/ocp_pose_ref.py | 68 +++++++++++++------ 5 files changed, 79 insertions(+), 66 deletions(-) diff --git a/agimus_controller/main/panda/main_hpp_mpc.py b/agimus_controller/main/panda/main_hpp_mpc.py index c4efb97f..3de9c48b 100644 --- a/agimus_controller/main/panda/main_hpp_mpc.py +++ b/agimus_controller/main/panda/main_hpp_mpc.py @@ -53,7 +53,7 @@ def main(self, use_gui=False, spawn_servers=False): self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel) start = time.time() - self.mpc.simulate_mpc(save_predictions=False) + self.mpc.simulate_mpc(save_predictions=True) end = time.time() print("Time of solving: ", end - start) costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True) diff --git a/agimus_controller/main/panda/main_reaching_goal.py b/agimus_controller/main/panda/main_reaching_goal.py index f20311cc..bcb9ab2d 100644 --- a/agimus_controller/main/panda/main_reaching_goal.py +++ b/agimus_controller/main/panda/main_reaching_goal.py @@ -47,12 +47,17 @@ def main(self, use_gui=False, spawn_servers=False): self.mpc.simulate_mpc(save_predictions=True) solver = self.mpc.ocp.solver costs = return_cost_vectors(solver, weighted=True) - constraint = return_constraint_vector(solver) + #constraint = return_constraint_vector(solver) plot_costs_from_dic(costs) - plot_constraints_from_dic(constraint) + #plot_constraints_from_dic(constraint) max_kkt = max(self.mpc.mpc_data["kkt_norm"]) + mean_kkt = np.mean(self.mpc.mpc_data["kkt_norm"]) + mean_iter = np.mean(self.mpc.mpc_data["nb_iter"]) + mean_solve_time = np.mean(self.mpc.mpc_data["solve_time"]) index = self.mpc.mpc_data["kkt_norm"].index(max_kkt) print(f"max kkt {max_kkt} index {index}") + print(f"mean kkt {mean_kkt} mean iter {mean_iter}") + print(f"mean solve time {mean_solve_time}") end = time.time() print("Time of solving: ", end - start) ee_frame_name = ocp_params.effector_frame_name diff --git a/agimus_controller/mpc.py b/agimus_controller/mpc.py index 961e40d5..ae0a2297 100644 --- a/agimus_controller/mpc.py +++ b/agimus_controller/mpc.py @@ -83,7 +83,7 @@ def simulate_mpc(self, save_predictions=False): next_node_idx = T if save_predictions: self.create_mpc_data(step_time=mpc_step_time) - for idx in range(1, self.whole_traj_T - 1): + for idx in range(1,self.whole_traj_T - 1): #self.whole_traj_T - 1 x_plan = self.update_planning(x_plan, self.whole_x_plan[next_node_idx, :]) a_plan = self.update_planning(a_plan, self.whole_a_plan[next_node_idx, :]) placement_ref = get_ee_pose_from_configuration( @@ -150,6 +150,8 @@ def create_mpc_data(self, step_time=None): self.mpc_data["control_refs"] = [u_ref] self.mpc_data["kkt_norm"] = [self.ocp.solver.KKT] self.mpc_data["nb_iter"] = [self.ocp.solver.iter] + self.mpc_data["nb_qp_iter"] = [self.ocp.solver.qp_iters] + if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() self.mpc_data["coll_residuals"] = collision_residuals @@ -166,6 +168,7 @@ def fill_predictions_and_refs_arrays(self, step_time=None): self.mpc_data["control_refs"].append(u_ref) self.mpc_data["kkt_norm"].append(self.ocp.solver.KKT) self.mpc_data["nb_iter"].append(self.ocp.solver.iter) + self.mpc_data["nb_qp_iter"].append(self.ocp.solver.qp_iters) if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index 01c10f38..9ab047e5 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 = 3e-2 + self._collision_margin = 2e-2 self.state = crocoddyl.StateMultibody(self._rmodel) self.actuation = crocoddyl.ActuationModelFull(self.state) self.nq = self._rmodel.nq # Number of joints of the robot @@ -128,21 +128,25 @@ def set_running_models(self): """Set running models based on state and acceleration reference trajectory.""" running_models = [] + placement_ref = get_ee_pose_from_configuration( + self._rmodel, self._rdata, self._effector_frame_id, self.x_plan[-1,: self.nq] + ) for idx in range(self.params.horizon_size - 1): running_cost_model = crocoddyl.CostModelSum(self.state) x_ref = self.x_plan[idx, :] - x_reg_cost = self.get_state_residual(x_ref) + activation_weights_x = np.array([1.0]*7+[0.1]*7) + x_reg_cost = self.get_state_residual(x_ref,activation_weights_x) u_reg_cost = self.get_control_residual(self.u_plan[idx, :]) vel_reg_cost = self.get_velocity_residual() - placement_ref = get_ee_pose_from_configuration( - self._rmodel, self._rdata, self._effector_frame_id, x_ref[: self.nq] - ) + #placement_ref = get_ee_pose_from_configuration( + # self._rmodel, self._rdata, self._effector_frame_id, x_ref[: self.nq] + #) placement_reg_cost = self.get_placement_residual(placement_ref) running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) running_cost_model.addCost("uReg", u_reg_cost, self._weight_u_reg) running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg) running_cost_model.addCost( - "gripperPose", placement_reg_cost, 0 + "gripperPose", placement_reg_cost, self._weight_ee_placement ) # useful for mpc to reset ocp if self.params.use_constraints: @@ -161,34 +165,6 @@ def set_running_models(self): self.running_models = running_models return self.running_models - def get_model(self, placement_ref, x_ref: np.ndarray, u_ref: np.ndarray): - running_cost_model = crocoddyl.CostModelSum(self.state) - x_reg_cost = self.get_state_residual(x_ref) - u_reg_cost = self.get_control_residual(u_ref) - vel_reg_cost = self.get_velocity_residual() - placement_ref = get_ee_pose_from_configuration( - self._rmodel, self._rdata, self._effector_frame_id, x_ref[: self.nq] - ) - placement_reg_cost = self.get_placement_residual(placement_ref) - running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) - running_cost_model.addCost("uReg", u_reg_cost, self._weight_u_reg) - running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg) - running_cost_model.addCost( - "gripperPose", placement_reg_cost, 0 - ) # useful for mpc to reset ocp - - if self.params.use_constraints: - constraints = self.get_constraints() - running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( - self.state, self.actuation, running_cost_model, constraints - ) - else: - running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( - self.state, self.actuation, running_cost_model - ) - running_DAM.armature = self.params.armature - return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt) - def get_constraints(self): constraint_model_manager = crocoddyl.ConstraintModelManager(self.state, self.nq) if len(self._cmodel.collisionPairs) != 0: @@ -249,16 +225,17 @@ def get_terminal_model_with_constraints( """Return terminal model with constraints for mim_solvers.""" terminal_cost_model = crocoddyl.CostModelSum(self.state) placement_reg_cost = self.get_placement_residual(placement_ref) - x_reg_cost = self.get_state_residual(x_ref) + activation_weights_x = np.array([1.0]*7+[0.1]*7) + x_reg_cost = self.get_state_residual(x_ref,activation_weights_x) u_reg_cost = self.get_control_residual(u_ref) vel_cost = self.get_velocity_residual() terminal_cost_model.addCost("xReg", x_reg_cost, 0) - if np.linalg.norm(x_ref[self.nq :]) < 1e-9: - terminal_cost_model.addCost("velReg", vel_cost, self._weight_ee_placement) - else: - terminal_cost_model.addCost("velReg", vel_cost, 0) + #if np.linalg.norm(x_ref[self.nq :]) < 1e-9: + # terminal_cost_model.addCost("velReg", vel_cost, self._weight_ee_placement) + #else: + terminal_cost_model.addCost("velReg", vel_cost, 0) terminal_cost_model.addCost( - "gripperPose", placement_reg_cost, self._weight_ee_placement + "gripperPose", placement_reg_cost, 0# self._weight_ee_placement ) terminal_cost_model.addCost("uReg", u_reg_cost, 0) @@ -436,6 +413,7 @@ def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): self.update_model( runningModels[node_idx], runningModels[node_idx + 1], True ) + self.update_cost(runningModels[node_idx], runningModels[-1], "gripperPose", False) self.update_model(runningModels[-1], self.solver.problem.terminalModel, False) if self.params.use_constraints: terminal_model = self.get_terminal_model_with_constraints( @@ -476,8 +454,9 @@ def run_solver(self, problem, xs_init, us_init): if self.params.use_constraints: solver = mim_solvers.SolverCSQP(problem) solver.use_filter_line_search = True - solver.termination_tolerance = 2e-4 + solver.termination_tolerance = 1e-4 solver.max_qp_iters = self.params.max_qp_iter + #solver.setCallbacks([mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]) solver.with_callbacks = self.params.activate_callback else: solver = crocoddyl.SolverFDDP(problem) diff --git a/agimus_controller/ocps/ocp_pose_ref.py b/agimus_controller/ocps/ocp_pose_ref.py index 0a8fca45..0165780c 100644 --- a/agimus_controller/ocps/ocp_pose_ref.py +++ b/agimus_controller/ocps/ocp_pose_ref.py @@ -112,7 +112,7 @@ def get_increasing_weight(self, time, max_weight): / self.params.increasing_weights["time_reach_percent"] ) - def get_model(self, x_ref, u_ref, des_pose): + def get_model(self, x_ref, u_ref, des_pose,add_constraints = True, null_weights=False): running_cost_model = crocoddyl.CostModelSum(self.state) u_reg_cost = self.get_control_residual(u_ref) ugrav_reg_cost = self.get_control_grav_residual() @@ -120,20 +120,35 @@ def get_model(self, x_ref, u_ref, des_pose): x_reg_cost = self.get_state_residual(x_ref, x_reg_weights) vel_reg_cost = self.get_velocity_residual() placement_reg_cost = self.get_placement_residual(des_pose) - running_cost_model.addCost("uReg", u_reg_cost, 0) - running_cost_model.addCost("ugravReg", ugrav_reg_cost, self._weight_u_reg) - running_cost_model.addCost( - "gripperPose", placement_reg_cost, self._weight_ee_placement - ) - running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg) - running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) + if null_weights: + running_cost_model.addCost("uReg", u_reg_cost, 0) + running_cost_model.addCost("ugravReg", ugrav_reg_cost, self._weight_u_reg) + running_cost_model.addCost( + "gripperPose", placement_reg_cost, 0 + ) + running_cost_model.addCost("velReg", vel_reg_cost, 0) + running_cost_model.addCost("xReg", x_reg_cost, 0) + else: + running_cost_model.addCost("uReg", u_reg_cost, 0) + running_cost_model.addCost("ugravReg", ugrav_reg_cost, self._weight_u_reg) + running_cost_model.addCost( + "gripperPose", placement_reg_cost, self._weight_ee_placement + ) + running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg) + running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) - constraints = self.get_constraints() - running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( - self.state, self.actuation, running_cost_model, constraints - ) + if add_constraints: + constraints = self.get_constraints() + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, running_cost_model, constraints + ) + else: + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, running_cost_model + ) running_DAM.armature = self.params.armature return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt) + def get_terminal_model(self, x_ref, u_ref, des_pose): cost_model = crocoddyl.CostModelSum(self.state) @@ -266,7 +281,10 @@ def reset_ocp(self, x0, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): self.solver.problem.x0 = x0 u_grav = self.get_grav_compensation(x0[: self.nq]) runningModels = list(self.solver.problem.runningModels) - for node_idx in range(len(runningModels) - 1): + self.update_model( + runningModels[0], runningModels[ 1], False + ) + for node_idx in range(1,len(runningModels) - 1): self.update_model( runningModels[node_idx], runningModels[node_idx + 1], True ) @@ -299,7 +317,10 @@ def build_ocp_from_plannif(self, x0): ) self.set_weight_ee_placement(weight) self.set_vel_weight(weight / 10) - models.append(self.get_model(self.x_goal, u_grav, self.des_pose)) + if idx ==0: + models.append(self.get_model(self.x_goal, u_grav, self.des_pose,add_constraints=False,null_weights=False)) + else: + models.append(self.get_model(self.x_goal, u_grav, self.des_pose,add_constraints=True)) terminal_model = self.get_terminal_model(x0, u_grav, self.des_pose) self.next_node_time = self.params.horizon_size * self.params.dt - 0.2 @@ -315,10 +336,15 @@ def run_solver(self, problem, xs_init, us_init): set_callback : activate solver callback """ # Creating the solver for this OC problem, defining a logger - solver = mim_solvers.SolverCSQP(problem) - solver.use_filter_line_search = True - solver.termination_tolerance = 1e-4 - solver.max_qp_iters = self.params.max_qp_iter - solver.with_callbacks = self.params.activate_callback - solver.solve(xs_init, us_init, self.params.max_iter) - self.solver = solver + if self.solver is None: + print("first solve") + solver = mim_solvers.SolverCSQP(problem) + solver.use_filter_line_search = True + solver.termination_tolerance = 1e-4 + solver.max_qp_iters = self.params.max_qp_iter + solver.with_callbacks = self.params.activate_callback + solver.solve(xs_init, us_init, self.params.max_iter) + self.solver = solver + else: + #self.solver.problem = problem + self.solver.solve(xs_init, us_init, self.params.max_iter) From a3946a79c84302642241493d49fd532b5d9a879e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Thu, 16 Jan 2025 08:22:05 +0000 Subject: [PATCH 07/10] add new main, slight change in scene file --- CMakeLists.txt | 2 +- README.md | 3 +- agimus_controller/hpp_panda/scenes.py | 8 +- .../main/panda/main_hpp_mpc_replay_ros.py | 99 +++++++++++++++++++ .../robot_model/obstacle_params_parser.py | 1 + 5 files changed, 107 insertions(+), 6 deletions(-) create mode 100644 agimus_controller/main/panda/main_hpp_mpc_replay_ros.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 09caabca..3d9b3950 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,7 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY) trajectory_buffer.py trajectory_point.py) set(pkg2 ${PROJECT_NAME}/hpp_panda planner.py scenes.py) set(pkg3 ${PROJECT_NAME}/main/panda main_hpp_mpc_buffer.py main_hpp_mpc.py - main_meshcat_display.py main_optim_traj.py main_scenes.py) + main_meshcat_display.py main_optim_traj.py main_scenes.py main_hpp_mpc_replay_ros.py) set(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py) set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp_pose_ref.py ocp.py) set(pkg6 ${PROJECT_NAME}/robot_model obstacle_params_parser.py panda_model.py diff --git a/README.md b/README.md index ea7a4f7e..658c68ca 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,8 @@ Different mains are available: - `python3 -m agimus_controller.main.ur3.main_hpp_mpc -N=1` - panda scripts : - `python3 -m agimus_controller.main.panda.main_hpp_mpc_buffer` - - `python3 -m agimus_controller.main.panda.main_hpp_mpc` + - `python3 -im agimus_controller.main.panda.main_hpp_mpc` + - `python3 -im agimus_controller.main.panda.main_hpp_mpc_replay_ros` - `python3 -im agimus_controller.main.panda.main_reaching_goal` - `python3 -m agimus_controller.main.panda.main_meshcat_display` - `python3 -m agimus_controller.main.panda.main_optim_traj` diff --git a/agimus_controller/hpp_panda/scenes.py b/agimus_controller/hpp_panda/scenes.py index 0cfd4ec8..bd651f6d 100644 --- a/agimus_controller/hpp_panda/scenes.py +++ b/agimus_controller/hpp_panda/scenes.py @@ -150,13 +150,13 @@ def get_shapes_avoiding_collision(self): """ if self._name_scene == "box" or "wall": self.shapes_avoiding_collision = [ - "panda_link5_sc_capsule_0", - "panda_leftfinger_0", + #"panda_link5_sc_capsule_0", + #"panda_leftfinger_0", ] elif self._name_scene == "ball": self.shapes_avoiding_collision = [ - "panda_link5_sc_capsule_0", - "panda_leftfinger_0", + #"panda_link5_sc_capsule_0", + #"panda_leftfinger_0", ] else: raise NotImplementedError( diff --git a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py new file mode 100644 index 00000000..02d17618 --- /dev/null +++ b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py @@ -0,0 +1,99 @@ +import time +import numpy as np +from agimus_controller.hpp_interface import HppInterface +from agimus_controller.mpc import MPC +from agimus_controller.utils.path_finder import get_package_path, get_mpc_params_dict +from agimus_controller.visualization.plots import MPCPlots +from agimus_controller.ocps.ocp_croco_hpp import OCPCrocoHPP +from agimus_controller.robot_model.panda_model import ( + PandaRobotModel, + PandaRobotModelParameters, +) +from agimus_controller.ocps.parameters import OCPParameters +from agimus_controller.main.servers import Servers + +from agimus_controller.utils.ocp_analyzer import ( + return_cost_vectors, + return_constraint_vector, + plot_costs_from_dic, + plot_constraints_from_dic, +) + + +class APP(object): + def main(self, use_gui=False, spawn_servers=False): + if spawn_servers: + self.servers = Servers() + self.servers.spawn_servers(use_gui) + + panda_params = PandaRobotModelParameters() + panda_params.collision_as_capsule = True + panda_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" + ) + pandawrapper = PandaRobotModel.load_model( + params=panda_params, env=collision_file_path + ) + mpc_params_dict = get_mpc_params_dict(task_name="pick_and_place") + ocp_params = OCPParameters() + ocp_params.set_parameters_from_dict(mpc_params_dict["ocp"]) + rmodel = pandawrapper.get_reduced_robot_model() + cmodel = pandawrapper.get_reduced_collision_model() + ee_frame_name = panda_params.ee_frame_name + + self.hpp_interface = HppInterface() + q_init, q_goal = self.hpp_interface.get_panda_q_init_q_goal() + self.hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui) + viewer = self.hpp_interface.get_viewer() + #x_plan, a_plan, _ = hpp_interface.get_hpp_x_a_planning(1e-2) + hpp_traj_dict = np.load("/home/gepetto/ros_ws/src/agimus_controller/agimus_controller/main/panda/" + "hpp_trajectory.npy",allow_pickle=True).item() + poses = hpp_traj_dict["poses"] + vels = hpp_traj_dict["vels"] + self.x_plan = np.concatenate([poses,vels],axis=1) + self.a_plan = hpp_traj_dict["accs"] + + ocp = OCPCrocoHPP(rmodel, cmodel, ocp_params) + + self.mpc = MPC(ocp, self.x_plan, self.a_plan, rmodel, cmodel) + + start = time.time() + self.mpc.simulate_mpc(save_predictions=True) + end = time.time() + print("Time of solving: ", end - start) + max_kkt = max(self.mpc.mpc_data["kkt_norm"]) + mean_kkt = np.mean(self.mpc.mpc_data["kkt_norm"]) + mean_iter = np.mean(self.mpc.mpc_data["nb_iter"]) + mean_solve_time = np.mean(self.mpc.mpc_data["step_time"]) + index = self.mpc.mpc_data["kkt_norm"].index(max_kkt) + print(f"max kkt {max_kkt} index {index}") + print(f"mean kkt {mean_kkt} mean iter {mean_iter}") + print(f"mean solve time {mean_solve_time}") + costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True) + constraint = return_constraint_vector(self.mpc.ocp.solver) + plot_costs_from_dic(costs) + plot_constraints_from_dic(constraint) + u_plan = self.mpc.ocp.get_u_plan(self.x_plan, self.a_plan) + self.mpc_plots = MPCPlots( + croco_xs=self.mpc.croco_xs, + croco_us=self.mpc.croco_us, + whole_x_plan=self.x_plan, + whole_u_plan=u_plan, + rmodel=rmodel, + vmodel=pandawrapper.get_reduced_visual_model(), + cmodel=cmodel, + DT=self.mpc.ocp.params.dt, + ee_frame_name=ee_frame_name, + viewer=viewer, + ) + return True + + +def main(): + return APP().main(use_gui=False, spawn_servers=False) + + +if __name__ == "__main__": + app = APP() + app.main(use_gui=True, spawn_servers=True) diff --git a/agimus_controller/robot_model/obstacle_params_parser.py b/agimus_controller/robot_model/obstacle_params_parser.py index 05ba4218..7d6635d7 100644 --- a/agimus_controller/robot_model/obstacle_params_parser.py +++ b/agimus_controller/robot_model/obstacle_params_parser.py @@ -195,6 +195,7 @@ def transform_model_into_capsules( 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", From 15e21ad69b7d1a52de507fa815d083833fc3bdbf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:22:24 +0000 Subject: [PATCH 08/10] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- CMakeLists.txt | 10 ++++- agimus_controller/hpp_panda/scenes.py | 8 ++-- .../main/panda/main_hpp_mpc_replay_ros.py | 10 +++-- .../main/panda/main_reaching_goal.py | 6 +-- agimus_controller/mpc.py | 16 ++++---- agimus_controller/ocps/ocp_croco_hpp.py | 41 +++++++++++-------- agimus_controller/ocps/ocp_pose_ref.py | 35 ++++++++++------ .../robot_model/obstacle_params_parser.py | 2 +- agimus_controller/utils/ocp_analyzer.py | 14 +++---- 9 files changed, 83 insertions(+), 59 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d9b3950..5940c299 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,8 +38,14 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY) set(pkg1 ${PROJECT_NAME} hpp_interface.py mpc_search.py mpc.py trajectory_buffer.py trajectory_point.py) set(pkg2 ${PROJECT_NAME}/hpp_panda planner.py scenes.py) - set(pkg3 ${PROJECT_NAME}/main/panda main_hpp_mpc_buffer.py main_hpp_mpc.py - main_meshcat_display.py main_optim_traj.py main_scenes.py main_hpp_mpc_replay_ros.py) + set(pkg3 + ${PROJECT_NAME}/main/panda + main_hpp_mpc_buffer.py + main_hpp_mpc.py + main_meshcat_display.py + main_optim_traj.py + main_scenes.py + main_hpp_mpc_replay_ros.py) set(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py) set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp_pose_ref.py ocp.py) set(pkg6 ${PROJECT_NAME}/robot_model obstacle_params_parser.py panda_model.py diff --git a/agimus_controller/hpp_panda/scenes.py b/agimus_controller/hpp_panda/scenes.py index bd651f6d..a624e9cc 100644 --- a/agimus_controller/hpp_panda/scenes.py +++ b/agimus_controller/hpp_panda/scenes.py @@ -150,13 +150,13 @@ def get_shapes_avoiding_collision(self): """ if self._name_scene == "box" or "wall": self.shapes_avoiding_collision = [ - #"panda_link5_sc_capsule_0", - #"panda_leftfinger_0", + # "panda_link5_sc_capsule_0", + # "panda_leftfinger_0", ] elif self._name_scene == "ball": self.shapes_avoiding_collision = [ - #"panda_link5_sc_capsule_0", - #"panda_leftfinger_0", + # "panda_link5_sc_capsule_0", + # "panda_leftfinger_0", ] else: raise NotImplementedError( diff --git a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py index 02d17618..4a87ff91 100644 --- a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py +++ b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py @@ -47,11 +47,15 @@ def main(self, use_gui=False, spawn_servers=False): q_init, q_goal = self.hpp_interface.get_panda_q_init_q_goal() self.hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui) viewer = self.hpp_interface.get_viewer() - #x_plan, a_plan, _ = hpp_interface.get_hpp_x_a_planning(1e-2) - hpp_traj_dict = np.load("/home/gepetto/ros_ws/src/agimus_controller/agimus_controller/main/panda/" + "hpp_trajectory.npy",allow_pickle=True).item() + # x_plan, a_plan, _ = hpp_interface.get_hpp_x_a_planning(1e-2) + hpp_traj_dict = np.load( + "/home/gepetto/ros_ws/src/agimus_controller/agimus_controller/main/panda/" + + "hpp_trajectory.npy", + allow_pickle=True, + ).item() poses = hpp_traj_dict["poses"] vels = hpp_traj_dict["vels"] - self.x_plan = np.concatenate([poses,vels],axis=1) + self.x_plan = np.concatenate([poses, vels], axis=1) self.a_plan = hpp_traj_dict["accs"] ocp = OCPCrocoHPP(rmodel, cmodel, ocp_params) diff --git a/agimus_controller/main/panda/main_reaching_goal.py b/agimus_controller/main/panda/main_reaching_goal.py index bcb9ab2d..46d272a6 100644 --- a/agimus_controller/main/panda/main_reaching_goal.py +++ b/agimus_controller/main/panda/main_reaching_goal.py @@ -13,9 +13,7 @@ from agimus_controller.main.servers import Servers from agimus_controller.utils.ocp_analyzer import ( return_cost_vectors, - return_constraint_vector, plot_costs_from_dic, - plot_constraints_from_dic, ) @@ -47,9 +45,9 @@ def main(self, use_gui=False, spawn_servers=False): self.mpc.simulate_mpc(save_predictions=True) solver = self.mpc.ocp.solver costs = return_cost_vectors(solver, weighted=True) - #constraint = return_constraint_vector(solver) + # constraint = return_constraint_vector(solver) plot_costs_from_dic(costs) - #plot_constraints_from_dic(constraint) + # plot_constraints_from_dic(constraint) max_kkt = max(self.mpc.mpc_data["kkt_norm"]) mean_kkt = np.mean(self.mpc.mpc_data["kkt_norm"]) mean_iter = np.mean(self.mpc.mpc_data["nb_iter"]) diff --git a/agimus_controller/mpc.py b/agimus_controller/mpc.py index ae0a2297..f583bf03 100644 --- a/agimus_controller/mpc.py +++ b/agimus_controller/mpc.py @@ -83,7 +83,7 @@ def simulate_mpc(self, save_predictions=False): next_node_idx = T if save_predictions: self.create_mpc_data(step_time=mpc_step_time) - for idx in range(1,self.whole_traj_T - 1): #self.whole_traj_T - 1 + for idx in range(1, self.whole_traj_T - 1): # self.whole_traj_T - 1 x_plan = self.update_planning(x_plan, self.whole_x_plan[next_node_idx, :]) a_plan = self.update_planning(a_plan, self.whole_a_plan[next_node_idx, :]) placement_ref = get_ee_pose_from_configuration( @@ -151,7 +151,7 @@ def create_mpc_data(self, step_time=None): self.mpc_data["kkt_norm"] = [self.ocp.solver.KKT] self.mpc_data["nb_iter"] = [self.ocp.solver.iter] self.mpc_data["nb_qp_iter"] = [self.ocp.solver.qp_iters] - + if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() self.mpc_data["coll_residuals"] = collision_residuals @@ -172,9 +172,9 @@ def fill_predictions_and_refs_arrays(self, step_time=None): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][ - coll_residual_key - ] += collision_residuals[coll_residual_key] + self.mpc_data["coll_residuals"][coll_residual_key] += ( + collision_residuals[coll_residual_key] + ) if step_time is not None: self.mpc_data["step_time"].append(step_time) @@ -204,7 +204,7 @@ def save_ocp_data(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][ - coll_residual_key - ] += collision_residuals[coll_residual_key] + self.mpc_data["coll_residuals"][coll_residual_key] += ( + collision_residuals[coll_residual_key] + ) np.save("ocp_data.npy", ocp_data) diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index 9ab047e5..f67e555e 100644 --- a/agimus_controller/ocps/ocp_croco_hpp.py +++ b/agimus_controller/ocps/ocp_croco_hpp.py @@ -129,18 +129,21 @@ def set_running_models(self): running_models = [] placement_ref = get_ee_pose_from_configuration( - self._rmodel, self._rdata, self._effector_frame_id, self.x_plan[-1,: self.nq] - ) + self._rmodel, + self._rdata, + self._effector_frame_id, + self.x_plan[-1, : self.nq], + ) for idx in range(self.params.horizon_size - 1): running_cost_model = crocoddyl.CostModelSum(self.state) x_ref = self.x_plan[idx, :] - activation_weights_x = np.array([1.0]*7+[0.1]*7) - x_reg_cost = self.get_state_residual(x_ref,activation_weights_x) + activation_weights_x = np.array([1.0] * 7 + [0.1] * 7) + x_reg_cost = self.get_state_residual(x_ref, activation_weights_x) u_reg_cost = self.get_control_residual(self.u_plan[idx, :]) vel_reg_cost = self.get_velocity_residual() - #placement_ref = get_ee_pose_from_configuration( + # placement_ref = get_ee_pose_from_configuration( # self._rmodel, self._rdata, self._effector_frame_id, x_ref[: self.nq] - #) + # ) placement_reg_cost = self.get_placement_residual(placement_ref) running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg) running_cost_model.addCost("uReg", u_reg_cost, self._weight_u_reg) @@ -225,17 +228,19 @@ def get_terminal_model_with_constraints( """Return terminal model with constraints for mim_solvers.""" terminal_cost_model = crocoddyl.CostModelSum(self.state) placement_reg_cost = self.get_placement_residual(placement_ref) - activation_weights_x = np.array([1.0]*7+[0.1]*7) - x_reg_cost = self.get_state_residual(x_ref,activation_weights_x) + activation_weights_x = np.array([1.0] * 7 + [0.1] * 7) + x_reg_cost = self.get_state_residual(x_ref, activation_weights_x) u_reg_cost = self.get_control_residual(u_ref) vel_cost = self.get_velocity_residual() terminal_cost_model.addCost("xReg", x_reg_cost, 0) - #if np.linalg.norm(x_ref[self.nq :]) < 1e-9: + # if np.linalg.norm(x_ref[self.nq :]) < 1e-9: # terminal_cost_model.addCost("velReg", vel_cost, self._weight_ee_placement) - #else: + # else: terminal_cost_model.addCost("velReg", vel_cost, 0) terminal_cost_model.addCost( - "gripperPose", placement_reg_cost, 0# self._weight_ee_placement + "gripperPose", + placement_reg_cost, + 0, # self._weight_ee_placement ) terminal_cost_model.addCost("uReg", u_reg_cost, 0) @@ -376,9 +381,11 @@ def get_inverse_dynamic_control(self, x, a): def update_cost(self, model, new_model, cost_name, update_weight=True): """Update model's cost reference and weight by copying new_model's cost.""" - model.differential.costs.costs[cost_name].cost.residual.reference = ( - new_model.differential.costs.costs[cost_name].cost.residual.reference.copy() - ) + model.differential.costs.costs[ + cost_name + ].cost.residual.reference = new_model.differential.costs.costs[ + cost_name + ].cost.residual.reference.copy() if update_weight: new_weight = new_model.differential.costs.costs[cost_name].weight model.differential.costs.costs[cost_name].weight = new_weight @@ -413,7 +420,9 @@ def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): self.update_model( runningModels[node_idx], runningModels[node_idx + 1], True ) - self.update_cost(runningModels[node_idx], runningModels[-1], "gripperPose", False) + self.update_cost( + runningModels[node_idx], runningModels[-1], "gripperPose", False + ) self.update_model(runningModels[-1], self.solver.problem.terminalModel, False) if self.params.use_constraints: terminal_model = self.get_terminal_model_with_constraints( @@ -456,7 +465,7 @@ def run_solver(self, problem, xs_init, us_init): solver.use_filter_line_search = True solver.termination_tolerance = 1e-4 solver.max_qp_iters = self.params.max_qp_iter - #solver.setCallbacks([mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]) + # solver.setCallbacks([mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]) solver.with_callbacks = self.params.activate_callback else: solver = crocoddyl.SolverFDDP(problem) diff --git a/agimus_controller/ocps/ocp_pose_ref.py b/agimus_controller/ocps/ocp_pose_ref.py index 0165780c..a5d180ca 100644 --- a/agimus_controller/ocps/ocp_pose_ref.py +++ b/agimus_controller/ocps/ocp_pose_ref.py @@ -112,7 +112,9 @@ def get_increasing_weight(self, time, max_weight): / self.params.increasing_weights["time_reach_percent"] ) - def get_model(self, x_ref, u_ref, des_pose,add_constraints = True, null_weights=False): + def get_model( + self, x_ref, u_ref, des_pose, add_constraints=True, null_weights=False + ): running_cost_model = crocoddyl.CostModelSum(self.state) u_reg_cost = self.get_control_residual(u_ref) ugrav_reg_cost = self.get_control_grav_residual() @@ -123,9 +125,7 @@ def get_model(self, x_ref, u_ref, des_pose,add_constraints = True, null_weights= if null_weights: running_cost_model.addCost("uReg", u_reg_cost, 0) running_cost_model.addCost("ugravReg", ugrav_reg_cost, self._weight_u_reg) - running_cost_model.addCost( - "gripperPose", placement_reg_cost, 0 - ) + running_cost_model.addCost("gripperPose", placement_reg_cost, 0) running_cost_model.addCost("velReg", vel_reg_cost, 0) running_cost_model.addCost("xReg", x_reg_cost, 0) else: @@ -148,7 +148,6 @@ def get_model(self, x_ref, u_ref, des_pose,add_constraints = True, null_weights= ) running_DAM.armature = self.params.armature return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt) - def get_terminal_model(self, x_ref, u_ref, des_pose): cost_model = crocoddyl.CostModelSum(self.state) @@ -281,10 +280,8 @@ def reset_ocp(self, x0, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): self.solver.problem.x0 = x0 u_grav = self.get_grav_compensation(x0[: self.nq]) runningModels = list(self.solver.problem.runningModels) - self.update_model( - runningModels[0], runningModels[ 1], False - ) - for node_idx in range(1,len(runningModels) - 1): + self.update_model(runningModels[0], runningModels[1], False) + for node_idx in range(1, len(runningModels) - 1): self.update_model( runningModels[node_idx], runningModels[node_idx + 1], True ) @@ -317,10 +314,22 @@ def build_ocp_from_plannif(self, x0): ) self.set_weight_ee_placement(weight) self.set_vel_weight(weight / 10) - if idx ==0: - models.append(self.get_model(self.x_goal, u_grav, self.des_pose,add_constraints=False,null_weights=False)) + if idx == 0: + models.append( + self.get_model( + self.x_goal, + u_grav, + self.des_pose, + add_constraints=False, + null_weights=False, + ) + ) else: - models.append(self.get_model(self.x_goal, u_grav, self.des_pose,add_constraints=True)) + models.append( + self.get_model( + self.x_goal, u_grav, self.des_pose, add_constraints=True + ) + ) terminal_model = self.get_terminal_model(x0, u_grav, self.des_pose) self.next_node_time = self.params.horizon_size * self.params.dt - 0.2 @@ -346,5 +355,5 @@ def run_solver(self, problem, xs_init, us_init): solver.solve(xs_init, us_init, self.params.max_iter) self.solver = solver else: - #self.solver.problem = problem + # self.solver.problem = problem self.solver.solve(xs_init, us_init, self.params.max_iter) diff --git a/agimus_controller/robot_model/obstacle_params_parser.py b/agimus_controller/robot_model/obstacle_params_parser.py index 7d6635d7..c9466517 100644 --- a/agimus_controller/robot_model/obstacle_params_parser.py +++ b/agimus_controller/robot_model/obstacle_params_parser.py @@ -195,7 +195,7 @@ def transform_model_into_capsules( def modify_colllision_model(self, rcmodel: pin.GeometryModel): geoms_to_remove_name = [ "panda_rightfinger_0", - 'panda_link7_sc_capsule_1', + "panda_link7_sc_capsule_1", "panda_link6_sc_capsule_0", "panda_link5_sc_capsule_0", "panda_link4_sc_capsule_0", diff --git a/agimus_controller/utils/ocp_analyzer.py b/agimus_controller/utils/ocp_analyzer.py index 8f38fb94..09df13a9 100644 --- a/agimus_controller/utils/ocp_analyzer.py +++ b/agimus_controller/utils/ocp_analyzer.py @@ -88,11 +88,9 @@ def return_cost_vectors(ddp, weighted=False, integrated=False): cost_tag ].cost if weighted: - costs[cost_tag][ - -1 - ] *= ddp.problem.terminalModel.differential.costs.costs.todict()[ - cost_tag - ].weight + costs[cost_tag][-1] *= ( + ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight + ) except Exception as ex: print(ex.with_traceback()) costs[cost_tag][-1] = np.mean( @@ -160,9 +158,9 @@ def return_weights(ddp): ): if cost_tag not in weights: weights.update({cost_tag: np.nan * np.ones(ddp.problem.T + 1)}) - weights[cost_tag][ - -1 - ] = ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight + weights[cost_tag][-1] = ( + ddp.problem.terminalModel.differential.costs.costs.todict()[cost_tag].weight + ) return weights From 4521e74232a9860ffbae44ab48e50e5ee30514e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Fri, 17 Jan 2025 16:14:07 +0000 Subject: [PATCH 09/10] slight changes in ocp --- .../main/panda/main_hpp_mpc_replay_ros.py | 4 +- agimus_controller/mpc.py | 14 ++-- agimus_controller/ocps/ocp_croco_hpp.py | 67 +++++++++++++++---- 3 files changed, 64 insertions(+), 21 deletions(-) diff --git a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py index 4a87ff91..013525ab 100644 --- a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py +++ b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py @@ -75,9 +75,9 @@ def main(self, use_gui=False, spawn_servers=False): print(f"mean kkt {mean_kkt} mean iter {mean_iter}") print(f"mean solve time {mean_solve_time}") costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True) - constraint = return_constraint_vector(self.mpc.ocp.solver) + # constraint = return_constraint_vector(self.mpc.ocp.solver) plot_costs_from_dic(costs) - plot_constraints_from_dic(constraint) + # plot_constraints_from_dic(constraint) u_plan = self.mpc.ocp.get_u_plan(self.x_plan, self.a_plan) self.mpc_plots = MPCPlots( croco_xs=self.mpc.croco_xs, diff --git a/agimus_controller/mpc.py b/agimus_controller/mpc.py index f583bf03..34a280c9 100644 --- a/agimus_controller/mpc.py +++ b/agimus_controller/mpc.py @@ -55,7 +55,7 @@ def get_predictions(self): def get_collision_residuals(self): constraints_residual_dict = self.ocp.solver.problem.runningDatas[ - 0 + 1 ].differential.constraints.constraints.todict() constraints_values = {} for constraint_key in constraints_residual_dict.keys(): @@ -172,9 +172,9 @@ def fill_predictions_and_refs_arrays(self, step_time=None): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][coll_residual_key] += ( - collision_residuals[coll_residual_key] - ) + self.mpc_data["coll_residuals"][ + coll_residual_key + ] += collision_residuals[coll_residual_key] if step_time is not None: self.mpc_data["step_time"].append(step_time) @@ -204,7 +204,7 @@ def save_ocp_data(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][coll_residual_key] += ( - collision_residuals[coll_residual_key] - ) + self.mpc_data["coll_residuals"][ + coll_residual_key + ] += collision_residuals[coll_residual_key] np.save("ocp_data.npy", ocp_data) diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index f67e555e..56f19e1d 100644 --- a/agimus_controller/ocps/ocp_croco_hpp.py +++ b/agimus_controller/ocps/ocp_croco_hpp.py @@ -31,10 +31,12 @@ def __init__( self._weight_ee_placement = self.params.gripper_weight self._weight_vel_reg = 0 self._collision_margin = 2e-2 + self.ratio_q_to_qdot_weight = 0.04 self.state = crocoddyl.StateMultibody(self._rmodel) self.actuation = crocoddyl.ActuationModelFull(self.state) self.nq = self._rmodel.nq # Number of joints of the robot self.nv = self._rmodel.nv # Dimension of the joint's speed vector of the robot + self.modulo_nb_active_cons = 1 self.x_plan = None self.a_plan = None self.u_plan = None @@ -137,7 +139,13 @@ def set_running_models(self): for idx in range(self.params.horizon_size - 1): running_cost_model = crocoddyl.CostModelSum(self.state) x_ref = self.x_plan[idx, :] - activation_weights_x = np.array([1.0] * 7 + [0.1] * 7) + joint_ratio = 1.0 + activation_weights_x = np.array( + [1.0] * 4 + + [joint_ratio] * 3 + + [self.ratio_q_to_qdot_weight] * 4 + + [self.ratio_q_to_qdot_weight * joint_ratio] * 3 + ) x_reg_cost = self.get_state_residual(x_ref, activation_weights_x) u_reg_cost = self.get_control_residual(self.u_plan[idx, :]) vel_reg_cost = self.get_velocity_residual() @@ -152,8 +160,9 @@ def set_running_models(self): "gripperPose", placement_reg_cost, self._weight_ee_placement ) # useful for mpc to reset ocp - if self.params.use_constraints: - constraints = self.get_constraints() + if self.params.use_constraints: # and idx % 3 in [1, 2] + # active = self.modulo_nb_active_cons + constraints = self.get_constraints(active=True) running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( self.state, self.actuation, running_cost_model, constraints ) @@ -168,7 +177,7 @@ def set_running_models(self): self.running_models = running_models return self.running_models - def get_constraints(self): + def get_constraints(self, active=True): constraint_model_manager = crocoddyl.ConstraintModelManager(self.state, self.nq) if len(self._cmodel.collisionPairs) != 0: for col_idx in range(len(self._cmodel.collisionPairs)): @@ -177,7 +186,7 @@ def get_constraints(self): ) # Adding the constraint to the constraint manager constraint_model_manager.addConstraint( - "col_term_" + str(col_idx), collision_constraint + "col_term_" + str(col_idx), collision_constraint, active=active ) return constraint_model_manager @@ -228,7 +237,13 @@ def get_terminal_model_with_constraints( """Return terminal model with constraints for mim_solvers.""" terminal_cost_model = crocoddyl.CostModelSum(self.state) placement_reg_cost = self.get_placement_residual(placement_ref) - activation_weights_x = np.array([1.0] * 7 + [0.1] * 7) + joint_ratio = 1.0 + activation_weights_x = np.array( + [1.0] * 4 + + [joint_ratio] * 3 + + [self.ratio_q_to_qdot_weight] * 4 + + [self.ratio_q_to_qdot_weight * joint_ratio] * 3 + ) x_reg_cost = self.get_state_residual(x_ref, activation_weights_x) u_reg_cost = self.get_control_residual(u_ref) vel_cost = self.get_velocity_residual() @@ -246,7 +261,7 @@ def get_terminal_model_with_constraints( # Add torque constraint # Joints torque limits given by the manufactor - constraints = self.get_constraints() + constraints = self.get_constraints(active=True) terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( self.state, self.actuation, terminal_cost_model, constraints ) @@ -379,13 +394,24 @@ def get_inverse_dynamic_control(self, x, a): """Return inverse dynamic control for a given state and acceleration.""" return pin.rnea(self._rmodel, self._rdata, x[: self.nq], x[self.nq :], a).copy() + def update_constraint(self, model, active): + const_names = list(model.differential.constraints.constraints.todict().keys()) + if active not in [True, False]: + print("active not true or false but ", active) + for const_name in const_names: + print( + "initial val ", + model.differential.constraints.constraints[const_name].active, + "new val", + active, + ) + model.differential.constraints.constraints[const_name].active = active + def update_cost(self, model, new_model, cost_name, update_weight=True): """Update model's cost reference and weight by copying new_model's cost.""" - model.differential.costs.costs[ - cost_name - ].cost.residual.reference = new_model.differential.costs.costs[ - cost_name - ].cost.residual.reference.copy() + model.differential.costs.costs[cost_name].cost.residual.reference = ( + new_model.differential.costs.costs[cost_name].cost.residual.reference.copy() + ) if update_weight: new_weight = new_model.differential.costs.costs[cost_name].weight model.differential.costs.costs[cost_name].weight = new_weight @@ -416,6 +442,9 @@ def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): """Reset ocp problem using next reference in state and control.""" self.solver.problem.x0 = x runningModels = list(self.solver.problem.runningModels) + self.modulo_nb_active_cons += 1 + if self.modulo_nb_active_cons > 2: + self.modulo_nb_active_cons = 0 for node_idx in range(len(runningModels) - 1): self.update_model( runningModels[node_idx], runningModels[node_idx + 1], True @@ -423,7 +452,21 @@ def reset_ocp(self, x, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): self.update_cost( runningModels[node_idx], runningModels[-1], "gripperPose", False ) + active = node_idx % 3 == self.modulo_nb_active_cons + # if node_idx == 1: + # print( + # "self.modulo_nb_active_cons", + # self.modulo_nb_active_cons, + # "active ", + # active, + # ) + + # self.update_constraint(runningModels[node_idx], active=active) self.update_model(runningModels[-1], self.solver.problem.terminalModel, False) + active = (len(runningModels) - 1) % 3 == self.modulo_nb_active_cons + # self.update_constraint(runningModels[-1], active=active) + + # print("modulo ", self.modulo_nb_active_cons) if self.params.use_constraints: terminal_model = self.get_terminal_model_with_constraints( placement_ref, x_ref, u_plan From 8bc0cea8f2212b6e3f88ddc63ea9f3dfdcb26a15 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:46:57 +0000 Subject: [PATCH 10/10] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../main/panda/main_hpp_mpc_replay_ros.py | 2 -- agimus_controller/mpc.py | 12 ++++++------ agimus_controller/ocps/ocp_croco_hpp.py | 8 +++++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py index 013525ab..a0fee83e 100644 --- a/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py +++ b/agimus_controller/main/panda/main_hpp_mpc_replay_ros.py @@ -14,9 +14,7 @@ from agimus_controller.utils.ocp_analyzer import ( return_cost_vectors, - return_constraint_vector, plot_costs_from_dic, - plot_constraints_from_dic, ) diff --git a/agimus_controller/mpc.py b/agimus_controller/mpc.py index 34a280c9..43097b5c 100644 --- a/agimus_controller/mpc.py +++ b/agimus_controller/mpc.py @@ -172,9 +172,9 @@ def fill_predictions_and_refs_arrays(self, step_time=None): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][ - coll_residual_key - ] += collision_residuals[coll_residual_key] + self.mpc_data["coll_residuals"][coll_residual_key] += ( + collision_residuals[coll_residual_key] + ) if step_time is not None: self.mpc_data["step_time"].append(step_time) @@ -204,7 +204,7 @@ def save_ocp_data(self): if self.ocp.params.use_constraints: collision_residuals = self.get_collision_residuals() for coll_residual_key in collision_residuals.keys(): - self.mpc_data["coll_residuals"][ - coll_residual_key - ] += collision_residuals[coll_residual_key] + self.mpc_data["coll_residuals"][coll_residual_key] += ( + collision_residuals[coll_residual_key] + ) np.save("ocp_data.npy", ocp_data) diff --git a/agimus_controller/ocps/ocp_croco_hpp.py b/agimus_controller/ocps/ocp_croco_hpp.py index 56f19e1d..598279bf 100644 --- a/agimus_controller/ocps/ocp_croco_hpp.py +++ b/agimus_controller/ocps/ocp_croco_hpp.py @@ -409,9 +409,11 @@ def update_constraint(self, model, active): def update_cost(self, model, new_model, cost_name, update_weight=True): """Update model's cost reference and weight by copying new_model's cost.""" - model.differential.costs.costs[cost_name].cost.residual.reference = ( - new_model.differential.costs.costs[cost_name].cost.residual.reference.copy() - ) + model.differential.costs.costs[ + cost_name + ].cost.residual.reference = new_model.differential.costs.costs[ + cost_name + ].cost.residual.reference.copy() if update_weight: new_weight = new_model.differential.costs.costs[cost_name].weight model.differential.costs.costs[cost_name].weight = new_weight