From 852b1d254c6e2f71ba1a9a46942198e36dbf821d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 20 Nov 2024 10:09:09 +0000 Subject: [PATCH 1/4] add ocp with main script --- .../main/panda/main_reaching_goal.py | 84 +++++ agimus_controller/ocps/ocp_pose_ref.py | 322 ++++++++++++++++++ 2 files changed, 406 insertions(+) create mode 100644 agimus_controller/main/panda/main_reaching_goal.py create mode 100644 agimus_controller/ocps/ocp_pose_ref.py diff --git a/agimus_controller/main/panda/main_reaching_goal.py b/agimus_controller/main/panda/main_reaching_goal.py new file mode 100644 index 00000000..7ee0ae95 --- /dev/null +++ b/agimus_controller/main/panda/main_reaching_goal.py @@ -0,0 +1,84 @@ +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_mpc_params_dict +from agimus_controller.visualization.plots import MPCPlots +from agimus_controller.ocps.ocp_pose_ref import OCPPoseRef +from agimus_controller.robot_model.panda_model import ( + get_task_models, + get_robot_constructor, +) +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) + + rmodel, cmodel, vmodel = get_task_models(task_name="reaching_goal") + self.robot_constructor = get_robot_constructor(task_name="reaching_goal") + mpc_params_dict = get_mpc_params_dict(task_name="reaching_goal") + ocp_params = OCPParameters() + ocp_params.set_parameters_from_dict(mpc_params_dict["ocp"]) + + hpp_interface = HppInterface() + q_init, q_goal = hpp_interface.get_panda_q_init_q_goal() + hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui) + viewer = hpp_interface.get_viewer() + x0 = q_init + [0] * 7 + length = 500 + x_plan = np.array(x0 * length) + x_plan = np.reshape(x_plan, (length, 14)) + a_plan = np.zeros((length, 7)) + ocp = OCPPoseRef(rmodel, cmodel, ocp_params, np.array(q_goal)) + + self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel) + start = time.time() + self.mpc.simulate_mpc(save_predictions=True) + solver = self.mpc.ocp.solver + costs = return_cost_vectors(solver, weighted=True) + constraint = return_constraint_vector(solver) + plot_costs_from_dic(costs) + plot_constraints_from_dic(constraint) + max_kkt = max(self.mpc.mpc_data["kkt_norm"]) + index = self.mpc.mpc_data["kkt_norm"].index(max_kkt) + print(f"max kkt {max_kkt} index {index}") + end = time.time() + print("Time of solving: ", end - start) + ee_frame_name = ocp_params.effector_frame_name + self.mpc_plots = MPCPlots( + croco_xs=self.mpc.croco_xs, + croco_us=self.mpc.croco_us, + whole_x_plan=x_plan, + whole_u_plan=np.zeros((length - 1, 7)), + rmodel=rmodel, + vmodel=vmodel, + cmodel=cmodel, + DT=self.mpc.ocp.params.dt, + ee_frame_name=ee_frame_name, + viewer=viewer, + ) + + if use_gui: + self.mpc_plots.display_path_gepetto_gui() + 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/ocps/ocp_pose_ref.py b/agimus_controller/ocps/ocp_pose_ref.py new file mode 100644 index 00000000..f75fad4c --- /dev/null +++ b/agimus_controller/ocps/ocp_pose_ref.py @@ -0,0 +1,322 @@ +import crocoddyl +import pinocchio as pin +import numpy as np +import mim_solvers +from colmpc import ResidualDistanceCollision + +from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration +from agimus_controller_ros.parameters import OCPParameters + + +class OCPPoseRef: + def __init__( + self, + rmodel: pin.Model, + cmodel: pin.GeometryModel, + params: OCPParameters, + q_goal: np.array, + ) -> None: + """Class to define the OCP linked witha HPP generated trajectory. + + Args: + rmodel (pin.Model): Pinocchio model of the robot. + cmodel (pin.GeometryModel): Pinocchio geometry model of the robot. Must have been convexified for the collisions to work. + params (OCPParameters) : parameters of the ocp. + """ + + self._rmodel = rmodel + self._cmodel = cmodel + self.params = params + self._rdata = self._rmodel.createData() + + self._effector_frame_id = self._rmodel.getFrameId(params.effector_frame_name) + self._weight_u_reg = params.control_weight + self._weight_ee_placement = None + self._weight_vel_reg = None + self._weight_x_reg = params.state_weight + 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 + self.nv = self._rmodel.nv # Dimension of the joint's speed vector of the robot + self.x_plan = None + self.a_plan = None + self.u_plan = None + self.running_models = None + self.terminal_model = None + self.solver = None + self.next_node_time = None + self.q_goal = q_goal + self.x_goal = np.concatenate([q_goal, np.array([0.0] * self.nq)]) + self.des_pose = get_ee_pose_from_configuration( + self._rmodel, + self._rdata, + self._effector_frame_id, + np.array(q_goal), + ) + + def get_grav_compensation( + self, + q: np.ndarray, + ) -> np.ndarray: + """Return the reference of control u_plan that compensates gravity.""" + pin.computeGeneralizedGravity( + self._rmodel, + self._rdata, + q, + ) + return self._rdata.g.copy() + + 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 set_weights( + self, + weight_ee_placement: float, + weight_x_reg: float, + weight_u_reg: float, + weight_vel_reg: float, + ): + """Set weights of the ocp. + + Args: + weight_ee_placement (float): Weight of the placement of the end effector with regards to the target. + weight_x_reg (float): Weight of the state regularization. + weight_u_reg (float): Weight of the control regularization. + weight_vel_reg (float): Weight of the velocity regularization. + """ + self._weight_ee_placement = weight_ee_placement + self._weight_x_reg = weight_x_reg + self._weight_u_reg = weight_u_reg + self._weight_vel_reg = weight_vel_reg + + def set_weight_ee_placement(self, weight_ee_placement: float): + """Set end effector weight of the ocp. + + Args: + weight_ee_placement (float): Weight of the placement of the end effector with regards to the target. + """ + self._weight_ee_placement = weight_ee_placement + + def set_vel_weight(self, weight_vel_reg): + self._weight_vel_reg = weight_vel_reg + + def set_control_weight(self, weight_u_reg): + self._weight_u_reg = weight_u_reg + + def get_increasing_weight(self, time, max_weight): + return max_weight * np.tanh( + max(0.0, time) + * np.arctanh(self.params.increasing_weights["percent"]) + / self.params.increasing_weights["time_reach_percent"] + ) + + def get_model(self, x_ref, u_ref, des_pose): + running_cost_model = crocoddyl.CostModelSum(self.state) + u_reg_cost = self.get_control_residual(u_ref) + ugrav_reg_cost = self.get_control_grav_residual() + x_reg_weights = np.array([1.0] * self.nq + [0.0] * self.nv) + 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) + + constraints = self.get_constraints() + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, running_cost_model, constraints + ) + 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) + u_reg_cost = self.get_control_residual(u_ref) + ugrav_reg_cost = self.get_control_grav_residual() + x_reg_cost = self.get_state_residual(x_ref) + vel_reg_cost = self.get_velocity_residual() + placement_reg_cost = self.get_placement_residual(des_pose) + cost_model.addCost("uReg", u_reg_cost, 0) + cost_model.addCost("ugravReg", ugrav_reg_cost, 0) + cost_model.addCost("gripperPose", placement_reg_cost, 0) + cost_model.addCost("velReg", vel_reg_cost, 0) + cost_model.addCost("xReg", x_reg_cost, 0) + running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics( + self.state, self.actuation, 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: + for col_idx in range(len(self._cmodel.collisionPairs)): + collision_constraint = self._get_collision_constraint( + col_idx, self._collision_margin + ) + # Adding the constraint to the constraint manager + constraint_model_manager.addConstraint( + "col_term_" + str(col_idx), collision_constraint + ) + return constraint_model_manager + + def _get_collision_constraint( + self, col_idx: int, collision_margin: float + ) -> "crocoddyl.ConstraintModelResidual": + """Returns the collision constraint that will be in the constraint model manager. + + Args: + col_idx (int): index of the collision pair. + collision_margin (float): Lower bound of the constraint, ie the collision margin. + + Returns: + _type_: _description_ + """ + obstacleDistanceResidual = ResidualDistanceCollision( + self.state, 7, self._cmodel, col_idx + ) + + # Creating the inequality constraint + constraint = crocoddyl.ConstraintModelResidual( + self.state, + obstacleDistanceResidual, + np.array([collision_margin]), + np.array([np.inf]), + ) + return constraint + + def get_placement_residual(self, placement_ref): + """Return placement residual with desired reference for end effector placement.""" + return crocoddyl.CostModelResidual( + self.state, + crocoddyl.ResidualModelFramePlacement( + self.state, self._effector_frame_id, placement_ref + ), + ) + + def get_velocity_residual(self): + """Return velocity residual of desired joint.""" + vref = pin.Motion.Zero() + return crocoddyl.CostModelResidual( + self.state, + crocoddyl.ResidualModelFrameVelocity( + self.state, + self._effector_frame_id, + vref, + pin.WORLD, + ), + ) + + def get_control_residual(self, uref): + """Return control residual with uref the control reference.""" + return crocoddyl.CostModelResidual( + self.state, crocoddyl.ResidualModelControl(self.state, uref) + ) + + def get_control_grav_residual(self): + """Return control residual with uref the control reference.""" + return crocoddyl.CostModelResidual( + self.state, crocoddyl.ResidualModelControlGrav(self.state) + ) + + def get_state_residual(self, xref, x_reg_weights=None): + """Return state residual with xref the state reference.""" + 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 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() + ) + if update_weight: + new_weight = new_model.differential.costs.costs[cost_name].weight + model.differential.costs.costs[cost_name].weight = new_weight + + def update_weight(self, model, new_model, cost_name): + new_weight = new_model.differential.costs.costs[cost_name].weight + model.differential.costs.costs[cost_name].weight = new_weight + + def update_model(self, model, new_model, update_weight): + """update model's costs by copying new_model's costs.""" + self.update_cost(model, new_model, "gripperPose", update_weight) + self.update_cost(model, new_model, "velReg", update_weight) + self.update_cost(model, new_model, "xReg", update_weight) + self.update_weight(model, new_model, "ugravReg") + + def reset_ocp(self, x0, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref): + """Reset ocp problem using next reference in state and control.""" + 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[node_idx], runningModels[node_idx + 1], True + ) + weight = self.get_increasing_weight( + self.next_node_time - self.params.dt, + self.params.increasing_weights["max"] / self.params.dt, + ) + self.set_weight_ee_placement(weight) + self.set_vel_weight(weight / 10) + last_running_model = self.get_model(self.x_goal, u_grav, self.des_pose) + self.update_model(runningModels[-1], last_running_model, True) + terminal_model = self.get_terminal_model(x0, u_grav, self.des_pose) + self.next_node_time += self.params.dt + self.update_model(self.solver.problem.terminalModel, terminal_model, True) + + def set_planning_variables(self, x_plan: np.ndarray, a_plan: np.ndarray): + self.x_plan = x_plan + self.a_plan = a_plan + u_grav = self.get_grav_compensation(x_plan[0, : self.nq]) + self.u_plan = np.array(list(u_grav) * (self.params.horizon_size - 1)) + self.u_plan = np.reshape(self.u_plan, (self.params.horizon_size - 1, 7)) + + def build_ocp_from_plannif(self, x0): + u_grav = self.u_plan[0, :] + models = [] + for idx in range(self.params.horizon_size - 1): + time = idx * self.params.dt - 0.2 + weight = self.get_increasing_weight( + time, self.params.increasing_weights["max"] / self.params.dt + ) + 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)) + 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 + + return crocoddyl.ShootingProblem(x0, models, terminal_model) + + def run_solver(self, problem, xs_init, us_init): + """ + Run FDDP or CSQP solver + problem : crocoddyl ocp problem. + xs_init : xs warm start. + us_init : us warm start. + max_iter : max number of iteration for the solver + 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 From 168f7b4c14f40523362da3647042cac960a765c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 20 Nov 2024 10:09:41 +0000 Subject: [PATCH 2/4] add ros node for new ocp --- .../reaching_goal_controller.py | 170 ++++++++++++++++++ launch/reaching_goal_controller.launch | 11 ++ node/reaching_goal_controller_node | 20 +++ 3 files changed, 201 insertions(+) create mode 100644 agimus_controller_ros/reaching_goal_controller.py create mode 100644 launch/reaching_goal_controller.launch create mode 100644 node/reaching_goal_controller_node diff --git a/agimus_controller_ros/reaching_goal_controller.py b/agimus_controller_ros/reaching_goal_controller.py new file mode 100644 index 00000000..da3d398f --- /dev/null +++ b/agimus_controller_ros/reaching_goal_controller.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +import rospy +import numpy as np +from copy import deepcopy +import time +from threading import Lock +from std_msgs.msg import Duration, Header +from linear_feedback_controller_msgs.msg import Control, Sensor +import atexit + +from agimus_controller.hpp_interface import HppInterface +from agimus_controller_ros.ros_np_multiarray import to_multiarray_f64 +from agimus_controller.robot_model.panda_model import get_task_models +from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration +from agimus_controller.mpc import MPC +from agimus_controller.ocps.ocp_pose_ref import OCPPoseRef +from agimus_controller_ros.sim_utils import convert_float_to_ros_duration_msg +from agimus_controller_ros.parameters import AgimusControllerNodeParameters + + +class ReachingGoalController: + def __init__(self, params: AgimusControllerNodeParameters) -> None: + self.params = params + self.rmodel, self.cmodel = get_task_models(task_name="goal_reaching") + self.rdata = self.rmodel.createData() + self.effector_frame_id = self.rmodel.getFrameId( + self.params.ocp.effector_frame_name + ) + self.hpp_interface = HppInterface() + self.nq = self.rmodel.nq + self.nv = self.rmodel.nv + self.nx = self.nq + self.nv + + q_init, q_goal = self.hpp_interface.get_panda_q_init_q_goal() + self.ocp = OCPPoseRef(self.rmodel, self.cmodel, params.ocp, np.array(q_goal)) + + if self.params.use_ros_params: + self.initialize_ros_attributes() + + def initialize_ros_attributes(self): + self.rate = rospy.Rate(self.params.rate, reset=True) + self.mutex = Lock() + self.sensor_msg = Sensor() + self.control_msg = Control() + self.state_subscriber = rospy.Subscriber( + "robot_sensors", Sensor, self.sensor_callback + ) + self.control_publisher = rospy.Publisher( + "motion_server_control", Control, queue_size=1, tcp_nodelay=True + ) + self.ocp_solve_time_pub = rospy.Publisher( + "ocp_solve_time", Duration, queue_size=1, tcp_nodelay=True + ) + self.ocp_x0_pub = rospy.Publisher( + "ocp_x0", Sensor, queue_size=1, tcp_nodelay=True + ) + self.first_robot_sensor_msg_received = False + self.first_pose_ref_msg_received = True + + def sensor_callback(self, sensor_msg): + with self.mutex: + self.sensor_msg = deepcopy(sensor_msg) + if not self.first_robot_sensor_msg_received: + self.first_robot_sensor_msg_received = True + + def wait_first_sensor_msg(self): + wait_for_input = True + while not rospy.is_shutdown() and wait_for_input: + wait_for_input = ( + not self.first_robot_sensor_msg_received + or not self.first_pose_ref_msg_received + ) + if wait_for_input: + rospy.loginfo_throttle(3, "Waiting until we receive a sensor message.") + rospy.loginfo_once("Start controller") + self.rate.sleep() + return wait_for_input + + def first_solve(self, x0): + # retrieve horizon state and acc references + # x0 = list(x0) + x_plan = np.array(list(x0) * self.params.ocp.horizon_size) + x_plan = np.reshape(x_plan, (self.params.ocp.horizon_size, 14)) + a_plan = np.zeros((self.params.ocp.horizon_size, 7)) + + # x0 = np.array(x0) + + # First solve + self.mpc = MPC(self.ocp, x_plan, a_plan, self.rmodel, self.cmodel) + self.mpc.ocp.set_planning_variables(x_plan, a_plan) + self.mpc.mpc_first_step( + x_plan, self.ocp.u_plan[: self.params.ocp.horizon_size - 1], x0 + ) + self.next_node_idx = self.params.ocp.horizon_size + if self.params.save_predictions_and_refs: + self.mpc.create_mpc_data() + _, u, k = self.mpc.get_mpc_output() + return u, k + + def solve(self, x0): + new_x_ref = np.array(x0) + new_a_ref = np.zeros((7)) + self.in_world_M_effector = get_ee_pose_from_configuration( + self.rmodel, + self.rdata, + self.effector_frame_id, + new_x_ref[: self.rmodel.nq], + ) + # if last point of the pick trajectory is in horizon and we wanna use vision pose + self.mpc.mpc_step(x0, new_x_ref, new_a_ref, self.in_world_M_effector) + + if self.params.save_predictions_and_refs: + self.mpc.fill_predictions_and_refs_arrays() + _, u, k = self.mpc.get_mpc_output() + return u, k + + def get_sensor_msg(self): + with self.mutex: + sensor_msg = deepcopy(self.sensor_msg) + return sensor_msg + + def send(self, sensor_msg, u, k): + self.control_msg.header = Header() + self.control_msg.header.stamp = rospy.Time.now() + self.control_msg.feedback_gain = to_multiarray_f64(k) + self.control_msg.feedforward = to_multiarray_f64(u) + self.control_msg.initial_state = sensor_msg + self.control_publisher.publish(self.control_msg) + + def exit_handler(self): + print("saving data") + np.save("mpc_params.npy", self.params.get_dict()) + if self.params.save_predictions_and_refs: + np.save("mpc_data.npy", self.mpc.mpc_data) + + def get_x0_from_sensor_msg(self, sensor_msg): + return np.concatenate( + [sensor_msg.joint_state.position, sensor_msg.joint_state.velocity] + ) + + def publish_ocp_solve_time(self, ocp_solve_time): + self.ocp_solve_time_pub.publish( + convert_float_to_ros_duration_msg(ocp_solve_time) + ) + + def run(self): + self.wait_first_sensor_msg() + sensor_msg = self.get_sensor_msg() + time.sleep(2.0) + start_compute_time = time.time() + u, k = self.first_solve(self.get_x0_from_sensor_msg(sensor_msg)) + compute_time = time.time() - start_compute_time + self.send(sensor_msg, u, k) + self.publish_ocp_solve_time(compute_time) + self.ocp_x0_pub.publish(sensor_msg) + self.rate.sleep() + atexit.register(self.exit_handler) + self.run_timer = rospy.Timer( + rospy.Duration(self.params.ocp.dt), self.run_callback + ) + rospy.spin() + + def run_callback(self, *args): + start_compute_time = time.time() + sensor_msg = self.get_sensor_msg() + u, k = self.solve(self.get_x0_from_sensor_msg(sensor_msg)) + self.send(sensor_msg, u, k) + compute_time = time.time() - start_compute_time + self.publish_ocp_solve_time(compute_time) + self.ocp_x0_pub.publish(sensor_msg) diff --git a/launch/reaching_goal_controller.launch b/launch/reaching_goal_controller.launch new file mode 100644 index 00000000..3c8d89a7 --- /dev/null +++ b/launch/reaching_goal_controller.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/node/reaching_goal_controller_node b/node/reaching_goal_controller_node new file mode 100644 index 00000000..1ca6a0e9 --- /dev/null +++ b/node/reaching_goal_controller_node @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 + +import rospy +from agimus_controller_ros.reaching_goal_controller import ReachingGoalController +from agimus_controller_ros.parameters import AgimusControllerNodeParameters + + +def run(): + rospy.init_node("talker", anonymous=True) + params = AgimusControllerNodeParameters() + params.set_parameters_from_ros() + node = ReachingGoalController(params) + node.run() + + +if __name__ == "__main__": + try: + run() + except rospy.ROSInterruptException: + pass From 43f339698ca370b95bfc157fc0b9e98e3d062b03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 20 Nov 2024 10:19:19 +0000 Subject: [PATCH 3/4] add new ocp and ros node in cmakelist --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b97d91e..c7dd73a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,7 +41,7 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY) 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) set(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py) - set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp.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 robot_model.py ur3_model.py) set(pkg7 ${PROJECT_NAME}/utils iostream.py ocp_analyzer.py pin_utils.py @@ -83,7 +83,8 @@ if(BUILD_ROS_INTERFACE) catkin_package(CATKIN_DEPENDS ${MY_CATKIN_DEPENDENCIES} DEPENDS ${MY_PYTHON_DEPENDENCIES}) catkin_install_python( - PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node + PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node + node/reaching_goal_controller_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(PROGRAMS node/hpp_corbaserver DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/node) From c086dcc30072b2f8bea5a4d7ac4ee5a6b2454ceb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 22 Nov 2024 09:49:55 +0000 Subject: [PATCH 4/4] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- CMakeLists.txt | 6 +++--- agimus_controller/main/panda/main_reaching_goal.py | 1 - agimus_controller/ocps/ocp_pose_ref.py | 8 +++++--- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c7dd73a6..09caabca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,9 +83,9 @@ if(BUILD_ROS_INTERFACE) catkin_package(CATKIN_DEPENDS ${MY_CATKIN_DEPENDENCIES} DEPENDS ${MY_PYTHON_DEPENDENCIES}) catkin_install_python( - PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node - node/reaching_goal_controller_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node + node/reaching_goal_controller_node DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION}) install(PROGRAMS node/hpp_corbaserver DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/node) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/agimus_controller/main/panda/main_reaching_goal.py b/agimus_controller/main/panda/main_reaching_goal.py index 7ee0ae95..f20311cc 100644 --- a/agimus_controller/main/panda/main_reaching_goal.py +++ b/agimus_controller/main/panda/main_reaching_goal.py @@ -79,6 +79,5 @@ def main(): if __name__ == "__main__": - app = APP() app.main(use_gui=True, spawn_servers=True) diff --git a/agimus_controller/ocps/ocp_pose_ref.py b/agimus_controller/ocps/ocp_pose_ref.py index f75fad4c..0a8fca45 100644 --- a/agimus_controller/ocps/ocp_pose_ref.py +++ b/agimus_controller/ocps/ocp_pose_ref.py @@ -241,9 +241,11 @@ def get_state_residual(self, xref, x_reg_weights=None): 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