From 359a2dd40f929c572a39841fcd958a0772242b8e Mon Sep 17 00:00:00 2001 From: EtienneAr Date: Wed, 28 Feb 2024 15:57:52 +0100 Subject: [PATCH 1/4] Remove foot tracking (unused and bugged anyway) and rename base_ref to base_vel_ref consistently --- config/walk_parameters.yaml | 1 - examples/run_jump_ocp.py | 6 +- examples/simple_ocp.py | 11 +- include/qrw/Estimator.hpp | 18 +-- include/qrw/IMPCWrapper.hpp | 2 +- include/qrw/IOCPAbstract.hpp | 2 +- python/expose-estimator.cpp | 3 - python/expose-mpc-interface.cpp | 6 +- python/expose-ocp-interface.cpp | 6 +- .../quadruped_reactive_walking/controller.py | 20 +-- .../ocp_defs/jump.py | 12 +- .../ocp_defs/walking.py | 52 ++------ .../tools/logger_control.py | 6 +- .../quadruped_reactive_walking/tools/utils.py | 30 ----- .../wb_mpc/ocp_crocoddyl.py | 16 +-- .../wb_mpc/ocp_proxddp.py | 14 +- .../wb_mpc/target.py | 123 ------------------ .../wb_mpc/task_spec.py | 2 - .../wbmpc_wrapper_multiprocess.py | 31 ++--- .../wbmpc_wrapper_ros.py | 22 ++-- .../wbmpc_wrapper_ros_mp.py | 13 +- .../wbmpc_wrapper_sync.py | 8 +- ros_qrw_msgs/MPCInit.srv | 5 +- ros_qrw_msgs/MPCSolve.srv | 5 +- src/Estimator.cpp | 23 +--- test_client.py | 6 +- 26 files changed, 87 insertions(+), 356 deletions(-) delete mode 100644 python/quadruped_reactive_walking/wb_mpc/target.py diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml index 70df35c6..c9d53d04 100644 --- a/config/walk_parameters.yaml +++ b/config/walk_parameters.yaml @@ -69,7 +69,6 @@ task: ground_collision_w: 1000. vertical_velocity_reg_w: 1. base_velocity_tracking_w: 800000. - foot_tracking_w: 0. impact_altitude_w: 1000000. impact_velocity_w: 100000. friction_cone_w: 0. diff --git a/examples/run_jump_ocp.py b/examples/run_jump_ocp.py index af49089d..af9135de 100644 --- a/examples/run_jump_ocp.py +++ b/examples/run_jump_ocp.py @@ -6,7 +6,6 @@ from quadruped_reactive_walking import Params from quadruped_reactive_walking.ocp_defs import jump -from quadruped_reactive_walking.wb_mpc.target import Target, make_footsteps_and_refs from crocoddyl import ShootingProblem from quadruped_reactive_walking.tools.kinematics_utils import get_translation_array from quadruped_reactive_walking.tools.meshcat_viewer import make_meshcat_viz @@ -14,10 +13,9 @@ params = Params.create_from_file() -target = Target(params) -footsteps, base_vel_refs = make_footsteps_and_refs(params, target) +base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(params.N_gait)] -ocp_spec = jump.JumpOCPBuilder(params, footsteps, base_vel_refs) +ocp_spec = jump.JumpOCPBuilder(params, base_vel_refs) robot = ocp_spec.task.robot rmodel = robot.model diff --git a/examples/simple_ocp.py b/examples/simple_ocp.py index 6a610a42..df28081a 100644 --- a/examples/simple_ocp.py +++ b/examples/simple_ocp.py @@ -9,7 +9,6 @@ from quadruped_reactive_walking.wb_mpc import AlgtrOCPProx, CrocOCP, OCP_TYPE_MAP from quadruped_reactive_walking.wb_mpc.task_spec import TaskSpec -from quadruped_reactive_walking.wb_mpc.target import Target, make_footsteps_and_refs print("OCP registered types:", pprint.pformat(OCP_TYPE_MAP), sep="\n") params = qrw.Params.create_from_file() @@ -18,14 +17,12 @@ print(task) -target = Target(params) - -footsteps, base_refs = make_footsteps_and_refs(params, target) +base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(params.N_gait)] x0 = task.x0 print("x0:", x0) -ocp = CrocOCP(params, footsteps, base_refs) +ocp = CrocOCP(params, base_vel_refs) nsteps = ocp.ddp.problem.T xs_i = [x0] * (nsteps + 1) @@ -37,7 +34,7 @@ print("============== PARALLEL ===================") -ocp2 = AlgtrOCPProx(params, footsteps, base_refs) +ocp2 = AlgtrOCPProx(params, base_vel_refs) ts = time.time() ocp2.solve(0) @@ -51,7 +48,7 @@ print("============== SERIAL ===================") -ocp3 = AlgtrOCPProx(params, footsteps, base_refs, aligator) +ocp3 = AlgtrOCPProx(params, base_vel_refs, aligator) ts = time.time() ocp3.solve(0) diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp index 2dc8ebe9..ea612ce1 100644 --- a/include/qrw/Estimator.hpp +++ b/include/qrw/Estimator.hpp @@ -32,7 +32,6 @@ class Estimator { /// states of the robot /// /// \param[in] gait Gait matrix that stores current and future contact status of the feet - /// \param[in] goals Target positions of the four feet /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated) /// \param[in] baseAngularVelocity Angular velocity of the IMU /// \param[in] baseOrientation Quaternion orientation of the IMU @@ -41,7 +40,6 @@ class Estimator { /// \param[in] perfectPosition Position of the robot in world frame /// \param[in] b_perfectVelocity Velocity of the robot in base frame void run(MatrixN const& gait, - MatrixN const& goals, VectorN const& baseLinearAcceleration, VectorN const& baseAngularVelocity, VectorN const& baseOrientation, @@ -62,10 +60,8 @@ class Estimator { VectorN getVEstimate() { return vEstimate_; } VectorN getVSecurity() { return vSecurity_; } VectorN getFeetStatus() { return feetStatus_; } - MatrixN getFeetTargets() { return feetTargets_; } Vector3 getBaseVelocityFK() { return baseVelocityFK_; } Vector3 getBasePositionFK() { return basePositionFK_; } - Vector3 getFeetPositionBarycenter() { return feetPositionBarycenter_; } Vector3 getBBaseVelocity() { return b_baseVelocity_; } Vector3 getFilterVelX() { return velocityFilter_.getX(); } @@ -107,22 +103,16 @@ class Estimator { void updateJointData(Vector12 const& q, Vector12 const& v); /// \brief Update the feet relative data - /// \details update feetStatus_, feetTargets_, feetStancePhaseDuration_ and + /// \details update feetStatus_, feetStancePhaseDuration_ and /// phaseRemainingDuration_ /// /// \param[in] gait Gait matrix that stores current and future contact status - /// of the feet \param[in] feetTargets Target positions of the four feet - void updateFeetStatus(MatrixN const& gait, MatrixN const& feetTargets); + /// of the feet + void updateFeetStatus(MatrixN const& gait); /// \brief Estimate base position and velocity using Forward Kinematics void updateForwardKinematics(); - /// \brief Compute barycenter of feet in contact - /// - /// \param[in] feet_status Contact status of the four feet - /// \param[in] goals Target positions of the four feet - void computeFeetPositionBarycenter(); - /// \brief Estimate the velocity of the base with forward kinematics using a /// contact point that /// is supposed immobile in world frame @@ -183,7 +173,6 @@ class Estimator { int phaseRemainingDuration_; //< Number of iterations left for the current gait phase Vector4 feetStancePhaseDuration_; //< Number of loops during which each foot has been in contact Vector4 feetStatus_; //< Contact status of the four feet - Matrix34 feetTargets_; //< Target positions of the four feet pinocchio::Model model_; //< Pinocchio models for frame computations and forward kinematics pinocchio::Data data_; //< Pinocchio datas for frame computations and forward kinematics @@ -192,7 +181,6 @@ class Estimator { Vector3 baseVelocityFK_; //< Base linear velocity estimated by Forward Kinematics Vector3 basePositionFK_; //< Base position estimated by Forward Kinematics Vector3 b_baseVelocity_; //< Filtered estimated velocity at center base (base frame) - Vector3 feetPositionBarycenter_; // Barycenter of feet in contact ComplementaryFilter positionFilter_; //< Complementary filter for base position ComplementaryFilter velocityFilter_; //< Complementary filter for base velocity diff --git a/include/qrw/IMPCWrapper.hpp b/include/qrw/IMPCWrapper.hpp index 7ce6df47..e2c23c7b 100644 --- a/include/qrw/IMPCWrapper.hpp +++ b/include/qrw/IMPCWrapper.hpp @@ -16,7 +16,7 @@ struct IMPCWrapper { int N_gait() const { return params_.N_gait; } uint window_size() const { return params_.window_size; } - virtual void solve(uint k, const ConstVecRefN &x0, Vector4 footstep, Motion base_vel_ref) = 0; + virtual void solve(uint k, const ConstVecRefN &x0, Motion base_vel_ref) = 0; virtual MPCResult get_latest_result() const = 0; }; diff --git a/include/qrw/IOCPAbstract.hpp b/include/qrw/IOCPAbstract.hpp index d321de2a..97714878 100644 --- a/include/qrw/IOCPAbstract.hpp +++ b/include/qrw/IOCPAbstract.hpp @@ -14,7 +14,7 @@ class IOCPAbstract { IOCPAbstract(Params const& params); virtual ~IOCPAbstract() = default; - virtual void push_node(uint k, const ConstVecRefN& x0, Matrix34 footsteps, Motion base_vel_ref) = 0; + virtual void push_node(uint k, const ConstVecRefN& x0, Motion base_vel_ref) = 0; virtual void solve(std::size_t k) = 0; Params params_; diff --git a/python/expose-estimator.cpp b/python/expose-estimator.cpp index 1414c4c7..4f87075b 100644 --- a/python/expose-estimator.cpp +++ b/python/expose-estimator.cpp @@ -21,10 +21,8 @@ struct EstimatorVisitor : public bp::def_visitor> { .def( "get_v_security", &Estimator::getVSecurity, bp::args("self"), "Get filtered velocity for security check.\n") .def("get_feet_status", &Estimator::getFeetStatus, bp::args("self")) - .def("get_feet_targets", &Estimator::getFeetTargets, bp::args("self")) .def("get_base_velocity_FK", &Estimator::getBaseVelocityFK, bp::args("self")) .def("get_base_position_FK", &Estimator::getBasePositionFK, bp::args("self")) - .def("get_feet_position_barycenter", &Estimator::getFeetPositionBarycenter, "") .def("get_b_base_velocity", &Estimator::getBBaseVelocity, "") .def("get_filter_vel_X", &Estimator::getFilterVelX, "") .def("get_filter_vel_DX", &Estimator::getFilterVelDX, "") @@ -49,7 +47,6 @@ struct EstimatorVisitor : public bp::def_visitor> { &Estimator::run, bp::args("self", "gait", - "goals", "baseLinearAcceleration", "baseAngularVelocity", "baseOrientation", diff --git a/python/expose-mpc-interface.cpp b/python/expose-mpc-interface.cpp index 8e32495f..70d4418c 100644 --- a/python/expose-mpc-interface.cpp +++ b/python/expose-mpc-interface.cpp @@ -8,9 +8,9 @@ struct PyMPCWrapper : MPCDerived, bp::wrapper { using MPCDerived::MPCDerived; using StdVecVecN = std::vector; - void solve(uint k, const ConstVecRefN &x0, Vector4 footstep, Motion base_vel_ref) override { + void solve(uint k, const ConstVecRefN &x0, Motion base_vel_ref) override { bp::override fn = this->get_override("solve"); - fn(k, x0, footstep, base_vel_ref); + fn(k, x0, base_vel_ref); } MPCResult get_latest_result() const override { return this->get_override("get_latest_result")(); } @@ -25,7 +25,7 @@ void exposeMPCInterface() { .def("get_latest_result", bp::pure_virtual(&PyMPCWrapper<>::get_latest_result), bp::args("self")) .def("solve", bp::pure_virtual(&PyMPCWrapper<>::solve), - (bp::arg("self"), bp::arg("k"), bp::arg("x0"), bp::arg("footstep"), bp::arg("base_vel_ref"))); + (bp::arg("self"), bp::arg("k"), bp::arg("x0"), bp::arg("base_vel_ref"))); } } // namespace qrw diff --git a/python/expose-ocp-interface.cpp b/python/expose-ocp-interface.cpp index ae6d5af9..947bad05 100644 --- a/python/expose-ocp-interface.cpp +++ b/python/expose-ocp-interface.cpp @@ -8,9 +8,9 @@ namespace qrw { struct OCPWrapper : IOCPAbstract, bp::wrapper { using IOCPAbstract::IOCPAbstract; - void push_node(uint k, const ConstVecRefN &x0, Matrix34 footsteps, Motion base_vel_ref) override { + void push_node(uint k, const ConstVecRefN &x0, Motion base_vel_ref) override { bp::override fn = get_override("push_node"); - fn(k, x0, footsteps, base_vel_ref); + fn(k, x0, base_vel_ref); } void solve(std::size_t k) override { @@ -30,7 +30,7 @@ void exposeSolverInterface() { .def("solve", bp::pure_virtual(&OCPWrapper::solve), bp::args("self", "k")) .def("push_node", bp::pure_virtual(&OCPWrapper::push_node), - bp::args("self", "k", "x0", "footsteps", "base_vel_ref"), + bp::args("self", "k", "x0", "base_vel_ref"), "Push a new node to the OCP.") .def("get_results", bp::pure_virtual(&OCPWrapper::get_results), diff --git a/python/quadruped_reactive_walking/controller.py b/python/quadruped_reactive_walking/controller.py index 724bf483..4172f20f 100644 --- a/python/quadruped_reactive_walking/controller.py +++ b/python/quadruped_reactive_walking/controller.py @@ -8,10 +8,9 @@ from crocoddyl import StateMultibody from . import wb_mpc -from .wb_mpc.target import Target, make_footsteps_and_refs from .wb_mpc.task_spec import TaskSpec from .wbmpc_wrapper_abstract import MPCResult -from .tools.utils import quaternionToRPY, make_initial_footstep +from .tools.utils import quaternionToRPY from typing import Type @@ -92,12 +91,9 @@ def __init__(self, params: qrw.Params, q_init, solver_cls: Type[wb_mpc.OCPAbstra self.result.q_des = self.task.q0[7:].copy() self.result.v_des = self.task.v0[6:].copy() - self.target = Target(params) - self.footsteps, self.base_refs = make_footsteps_and_refs(self.params, self.target) + self.base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(self.params.N_gait)] - self.default_footstep = make_initial_footstep(params.q_init) self.target_base = pin.Motion.Zero() - self.target_footstep = np.zeros((3, 4)) self.mpc = self._create_mpc(solver_cls=solver_cls) assert self.mpc is not None, "Error while instanciating MPCWrapper" @@ -127,11 +123,11 @@ def _create_mpc(self, solver_cls): if self.params.asynchronous_mpc: from .wbmpc_wrapper_ros_mp import ROSMPCAsyncClient - return ROSMPCAsyncClient(self.params, self.footsteps, self.base_refs, solver_cls) + return ROSMPCAsyncClient(self.params, self.base_vel_refs, solver_cls) else: from .wbmpc_wrapper_ros import ROSMPCWrapperClient - return ROSMPCWrapperClient(self.params, self.footsteps, self.base_refs, solver_cls, True) + return ROSMPCWrapperClient(self.params, self.base_vel_refs, solver_cls, True) else: if self.params.asynchronous_mpc: from .wbmpc_wrapper_multiprocess import ( @@ -141,8 +137,7 @@ def _create_mpc(self, solver_cls): from .wbmpc_wrapper_sync import SyncMPCWrapper as MPCWrapper return MPCWrapper( self.params, - self.footsteps, - self.base_refs, + self.base_vel_refs, solver_cls=solver_cls, ) @@ -167,10 +162,8 @@ def compute(self, device, qc=None): if self.params.movement == "base_circle" or self.params.movement == "walk": self.target_base.np[:] = self.v_ref - self.target_footstep[:] = 0.0 else: self.target_base.np[:] = 0.0 - self.target_footstep[:] = self.target.compute(self.k + self.params.N_gait * self.params.mpc_wbc_ratio) if self.k % self.params.mpc_wbc_ratio == 0: if self.mpc_solved: @@ -184,7 +177,7 @@ def compute(self, device, qc=None): try: self.t_mpc_start = time.time() - self.mpc.solve(self.k, x, self.target_footstep.copy(), self.target_base.copy()) + self.mpc.solve(self.k, x, self.target_base.copy()) except ValueError: import traceback @@ -361,7 +354,6 @@ def run_estimator(self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zero self.estimator.run( self.gait, - self.default_footstep, device.imu.linear_acceleration, device.imu.gyroscope, device.imu.attitude_euler, diff --git a/python/quadruped_reactive_walking/ocp_defs/jump.py b/python/quadruped_reactive_walking/ocp_defs/jump.py index 91967c0e..87580fc5 100644 --- a/python/quadruped_reactive_walking/ocp_defs/jump.py +++ b/python/quadruped_reactive_walking/ocp_defs/jump.py @@ -8,21 +8,19 @@ from quadruped_reactive_walking import Params from crocoddyl import CostModelSum from . import walking -from ..tools.utils import make_initial_footstep class JumpOCPBuilder: - def __init__(self, params: Params, footsteps, base_vel_refs): + def __init__(self, params: Params, base_vel_refs): self.params = params - self._base_builder = walking.WalkingOCPBuilder(params, footsteps, base_vel_refs) + self._base_builder = walking.WalkingOCPBuilder(params, base_vel_refs) self.task = self._base_builder.task self.state = self._base_builder.state self.rdata = self._base_builder.rdata self.x0 = self.task.x0 self.jump_spec = params.task["jump"] - feet_pos = make_initial_footstep(params.q_init) - self.ground_models_1 = self.create_ground_models(feet_pos) + self.ground_models_1 = self.create_ground_models() jump_vel = np.asarray(self.jump_spec["jump_velocity"]) jump_vel = pin.Motion(jump_vel) self.jump_models = self.create_jump_model(jump_vel) @@ -47,11 +45,11 @@ def build_timeline(self): assert len(rms) == N return rms, ground_tm - def create_ground_models(self, feet_pos): + def create_ground_models(self): rms = [] support_feet = np.asarray(self.task.feet_ids) for k in range(self.params.N_gait): - m = self._base_builder.make_running_model(support_feet, [], feet_pos, None) + m = self._base_builder.make_running_model(support_feet, [], None) rms.append(m) return rms, self._base_builder.make_terminal_model(support_feet) diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py index 4f6c9d24..8e9c4d63 100644 --- a/python/quadruped_reactive_walking/ocp_defs/walking.py +++ b/python/quadruped_reactive_walking/ocp_defs/walking.py @@ -23,7 +23,7 @@ class WalkingOCPBuilder(OCPBuilder): """Builder class to define the walking OCP.""" - def __init__(self, params: Params, footsteps, base_vel_refs): + def __init__(self, params: Params, base_vel_refs): super().__init__(params) self.task = task_spec.TaskSpec(params) self.state = StateMultibody(self.rmodel) @@ -38,7 +38,7 @@ def __init__(self, params: Params, footsteps, base_vel_refs): axis=0, ) - self.life_rm, self.life_tm = self.initialize_models_from_gait(self.life_gait, footsteps, base_vel_refs) + self.life_rm, self.life_tm = self.initialize_models_from_gait(self.life_gait, base_vel_refs) self.start_rm, self.start_tm = self.initialize_models_from_gait(self.starting_gait) self.end_rm, self.end_tm = self.initialize_models_from_gait(self.ending_gait) @@ -79,22 +79,18 @@ def select_next_model(self, k, current_gait, base_vel_ref): def rmodel(self): return self.task.model - def initialize_models_from_gait(self, gait, footsteps=None, base_vel_refs=None): + def initialize_models_from_gait(self, gait, base_vel_refs=None): """Create action models (problem stages) from a gait matrix and other optional data.""" # both or neither must be none - assert (footsteps is None) == (base_vel_refs is None) - if footsteps is not None: - assert len(footsteps) == len(base_vel_refs) running_models = [] feet_ids = np.asarray(self.task.feet_ids) for t in range(gait.shape[0]): support_feet_ids = feet_ids[gait[t] == 1] - feet_pos = get_active_feet(footsteps[t], support_feet_ids) if footsteps is not None else [] base_vel_ref = base_vel_refs[t] if base_vel_refs is not None else None has_switched = np.any(gait[t] != gait[t - 1]) switch_matrix = gait[t] if has_switched else np.array([]) switch_feet = feet_ids[switch_matrix == 1] - running_models.append(self.make_running_model(support_feet_ids, switch_feet, feet_pos, base_vel_ref)) + running_models.append(self.make_running_model(support_feet_ids, switch_feet, base_vel_ref)) support_feet_ids = feet_ids[gait[-1] == 1] terminal_model = self.make_terminal_model(support_feet_ids) @@ -151,7 +147,6 @@ def make_running_model( self, support_feet, switch_feet, - feet_pos: List[np.ndarray], base_vel_ref: Optional[pin.Motion], ): """ @@ -164,8 +159,6 @@ def make_running_model( self._add_friction_cost(i, support_feet, costs) self._add_force_reg(i, model) - if self.has_foot_track_cost: - self._add_foot_track_cost(i, costs) if self.has_ground_collision: self._add_ground_coll_penalty(i, costs, start_pos) if self.has_fly_high: @@ -185,7 +178,7 @@ def make_running_model( self._add_control_costs(costs) - self.update_tracking_costs(costs, feet_pos, base_vel_ref, support_feet) + self.update_tracking_costs(costs, base_vel_ref, support_feet) return model def _add_control_costs(self, costs: CostModelSum): @@ -236,19 +229,6 @@ def _add_force_reg(self, i: int, m: DifferentialActionModelContactFwdDynamics): costs.addCost(name, force_reg, self.task.force_reg_w) costs.changeCostStatus(name, False) - @property - def has_foot_track_cost(self): - return self.task.foot_tracking_w > 0 - - def _add_foot_track_cost(self, i: int, costs: CostModelSum): - nu = costs.nu - # Tracking foot trajectory - name = self.rmodel.frames[i].name + "_foot_tracking" - residual = crocoddyl.ResidualModelFrameTranslation(self.state, i, np.zeros(3), nu) - foot_tracking = CostModelResidual(self.state, residual) - costs.addCost(name, foot_tracking, self.task.foot_tracking_w) - costs.changeCostStatus(name, False) - def _add_ground_coll_penalty(self, i: int, costs: CostModelSum, start_pos): nu = costs.nu @@ -369,7 +349,6 @@ def _add_base_vel_cost(self, vel_ref: pin.Motion, costs: CostModelSum): def update_model( self, model, - feet_pos, base_vel_ref: Optional[pin.Motion], support_feet, is_terminal=False, @@ -379,18 +358,11 @@ def update_model( name = self.rmodel.frames[i].name + "_contact" model.differential.contacts.changeContactStatus(name, i in support_feet) if not is_terminal: - self.update_tracking_costs(model.differential.costs, feet_pos, base_vel_ref, support_feet) + self.update_tracking_costs(model.differential.costs, base_vel_ref, support_feet) - def update_tracking_costs(self, costs, feet_pos: List[np.ndarray], base_vel_ref: pin.Motion, support_feet): + def update_tracking_costs(self, costs, base_vel_ref: pin.Motion, support_feet): index = 0 for i in self.task.feet_ids: - if self.has_foot_track_cost: - name = "{}_foot_tracking".format(self.rmodel.frames[i].name) - if i in support_feet: - costs.costs[name].cost.residual.reference = feet_pos[index] - index += 1 - costs.changeCostStatus(name, i not in support_feet) - name = "{}_forceReg".format(self.rmodel.frames[i].name) costs.changeCostStatus(name, i in support_feet) @@ -407,12 +379,4 @@ def update_tracking_costs(self, costs, feet_pos: List[np.ndarray], base_vel_ref: if base_vel_ref is not None and self.has_base_vel_cost: name = "base_velocity_tracking" - costs.costs[name].cost.residual.reference = base_vel_ref - - -def get_active_feet(footstep, support_feet) -> List[np.ndarray]: - """Get the positions for all the active feet.""" - feet_pos = [] - for i, fid in enumerate(support_feet): - feet_pos.append(footstep[:, i]) - return feet_pos + costs.costs[name].cost.residual.reference = base_vel_ref \ No newline at end of file diff --git a/python/quadruped_reactive_walking/tools/logger_control.py b/python/quadruped_reactive_walking/tools/logger_control.py index f33c3a50..75126fca 100644 --- a/python/quadruped_reactive_walking/tools/logger_control.py +++ b/python/quadruped_reactive_walking/tools/logger_control.py @@ -160,11 +160,9 @@ def sample(self, controller: Controller, device, qualisys=None): if self.i == 0: for i in range(self.params.N_gait * self.params.mpc_wbc_ratio): - self.target[i] = controller.footsteps[i // self.params.mpc_wbc_ratio][:, 1] - self.target_base_linear[i] = controller.base_refs[i // self.params.mpc_wbc_ratio].linear - self.target_base_angular[i] = controller.base_refs[i // self.params.mpc_wbc_ratio].angular + self.target_base_linear[i] = controller.base_vel_refs[i // self.params.mpc_wbc_ratio].linear + self.target_base_angular[i] = controller.base_vel_refs[i // self.params.mpc_wbc_ratio].angular if self.i + self.params.N_gait * self.params.mpc_wbc_ratio < self.log_size: - self.target[self.i + self.params.N_gait * self.params.mpc_wbc_ratio] = controller.target_footstep[:, 1] self.target_base_linear[self.i + self.params.N_gait * self.params.mpc_wbc_ratio] = controller.v_ref[:][:3] self.target_base_angular[self.i + self.params.N_gait * self.params.mpc_wbc_ratio] = controller.v_ref[:][3:] diff --git a/python/quadruped_reactive_walking/tools/utils.py b/python/quadruped_reactive_walking/tools/utils.py index 86277d07..a4b16c2d 100644 --- a/python/quadruped_reactive_walking/tools/utils.py +++ b/python/quadruped_reactive_walking/tools/utils.py @@ -7,36 +7,6 @@ except ImportError: from multiprocessing.shared_memory import SharedMemory - -def make_initial_footstep(q_init): - import example_robot_data as erd - - # Load robot model and data - solo = erd.load("solo12") - q = solo.q0.reshape((-1, 1)) - q[7:, 0] = q_init - - pin.framesForwardKinematics(solo.model, solo.data, q) - - # Initialisation of model quantities - pin.updateFramePlacements(solo.model, solo.data) - - LEGS = ["FL", "FR", "HL", "HR"] - - # Initialisation of the position of footsteps - initial_footsteps = np.zeros((3, 4)) - h_init = 0.0 - indexes = [solo.model.getFrameId(leg + "_FOOT") for leg in LEGS] - for i in range(4): - initial_footsteps[:, i] = solo.data.oMf[indexes[i]].translation - h = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2] - if h > h_init: - h_init = h - initial_footsteps[2, :] = 0.0 - - return initial_footsteps - - def quaternionToRPY(quat): """ Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1) diff --git a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py index 8c466838..2afdcf7c 100644 --- a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py +++ b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py @@ -8,22 +8,18 @@ from typing import Optional from quadruped_reactive_walking import Params from . import task_spec -from ..ocp_defs.walking import ( - WalkingOCPBuilder, - get_active_feet, -) - +from ..ocp_defs.walking import WalkingOCPBuilder class CrocOCP(OCPAbstract): """ Generate a Crocoddyl OCP for the control task. """ - def __init__(self, params: Params, footsteps, base_refs): + def __init__(self, params: Params, base_vel_refs): super().__init__(params) self.task = task_spec.TaskSpec(params) - self._builder = WalkingOCPBuilder(params, footsteps, base_refs) + self._builder = WalkingOCPBuilder(params, base_vel_refs) self.rdata = self._builder.rdata self.current_gait = self._builder.current_gait @@ -74,13 +70,12 @@ def solve(self, k): self.t_solve = time.time() - t_start self.num_iters = self.ddp.iter - def push_node(self, k, x0, footsteps, base_vel_ref: Optional[pin.Motion]): + def push_node(self, k, x0, base_vel_ref: Optional[pin.Motion]): """ Create a shooting problem for a simple walking gait. :param k: current MPC iteration :param x0: initial condition - :param footstep: 2D array :param base_pose: 1D array """ self.x0 = x0 @@ -93,8 +88,7 @@ def push_node(self, k, x0, footsteps, base_vel_ref: Optional[pin.Motion]): return model, support_feet, base_vel_ref = self._builder.select_next_model(k, self.current_gait, base_vel_ref) - active_feet_pos = get_active_feet(footsteps, support_feet) - self._builder.update_model(model, active_feet_pos, base_vel_ref, support_feet) + self._builder.update_model(model, base_vel_ref, support_feet) self.circular_append(model) self.cycle_warm_start() diff --git a/python/quadruped_reactive_walking/wb_mpc/ocp_proxddp.py b/python/quadruped_reactive_walking/wb_mpc/ocp_proxddp.py index 6b7426b1..b2ec1393 100644 --- a/python/quadruped_reactive_walking/wb_mpc/ocp_proxddp.py +++ b/python/quadruped_reactive_walking/wb_mpc/ocp_proxddp.py @@ -32,10 +32,9 @@ def get_type_str(): def __init__( self, params: Params, - footsteps, - base_refs, + base_vel_refs, ): - super().__init__(params, footsteps, base_refs) + super().__init__(params, base_vel_refs) self.algtr_problem: aligator.TrajOptProblem = aligator.croc.convertCrocoddylProblem(self.croc_problem) @@ -110,12 +109,11 @@ class AlgtrOCPFDDP(AlgtrOCPAbstract): def __init__( self, params: Params, - footsteps, - base_refs, + base_vel_refs, ): print(Fore.BLUE + "[using SolverFDDP]" + Fore.RESET) self.solver = aligator.SolverFDDP(params.ocp.tol) - super().__init__(params, footsteps, base_refs) + super().__init__(params, base_vel_refs) def get_type_str(): return "algtr-fddp" @@ -124,13 +122,13 @@ def get_type_str(): class AlgtrOCPProx(AlgtrOCPAbstract): """Solve the OCP using aligator.""" - def __init__(self, params: Params, footsteps, base_refs): + def __init__(self, params: Params, base_vel_refs): print(Fore.GREEN + "[using SolverProxDDP]" + Fore.RESET) mu_init = 1e-11 self.solver = aligator.SolverProxDDP(params.ocp.tol, mu_init) self.solver.force_initial_condition = True self.solver.rollout_type = aligator.ROLLOUT_LINEAR - super().__init__(params, footsteps, base_refs) + super().__init__(params, base_vel_refs) def get_type_str(): return "algtr-prox" diff --git a/python/quadruped_reactive_walking/wb_mpc/target.py b/python/quadruped_reactive_walking/wb_mpc/target.py deleted file mode 100644 index ac29f51f..00000000 --- a/python/quadruped_reactive_walking/wb_mpc/target.py +++ /dev/null @@ -1,123 +0,0 @@ -import numpy as np -from scipy.interpolate import KroghInterpolator -from ..tools.utils import make_initial_footstep -import pinocchio as pin - - -class Target: - """Utility class to compute references for base and footstep.""" - - def __init__(self, params): - self.params = params - self.dt_wbc = params.dt_wbc - self.k_per_step = 160 - self.initial_delay = 1000 - self.initial_footsteps = make_initial_footstep(self.params.q_init) - self.base_vel_ref = pin.Motion() - - if params.movement == "base_circle": - self.initial_base = params.pose_init[:3] - self.A = np.array([0.02, 0.015, 0.0]) - self.offset = np.array([0.0, 0.0, 0.0]) - self.freq = np.array([0.5, 0.5, 0.0]) - self.phase = np.array([0.0, 0.0, 0.0]) - elif params.movement == "walk": - self.velocity_lin_target = np.array([0.5, 0, 0]) - self.velocity_ang_target = np.array([0, 0, 0]) - # dim 6 - self.base_vel_ref = pin.Motion(self.velocity_lin_target, self.velocity_ang_target) - elif params.movement == "circle": - self.A = np.array([0.05, 0.0, 0.04]) - self.offset = np.array([0.05, 0, 0.05]) - self.freq = np.array([0.5, 0.0, 0.5]) - self.phase = np.array([-np.pi / 2, 0.0, -np.pi / 2]) - elif params.movement == "step": - foot_pose = self.initial_footsteps[:, 1] - self.p0 = foot_pose - self.p1 = foot_pose + np.array([0.025, 0.0, 0.03]) - self.v1 = np.array([0.5, 0.0, 0.0]) - self.p2 = foot_pose + np.array([0.05, 0.0, 0.0]) - - self.T = self.k_per_step * self.dt_wbc - self.ts = np.repeat(np.linspace(0, self.T, 3), 2) - - self.update_time = -1 - else: - self.ramp_length = 100 - self.target_ramp_x = np.linspace(0.0, 0.0, self.ramp_length) - self.target_ramp_y = np.linspace(0.0, 0.0, self.ramp_length) - self.target_ramp_z = np.linspace(0.0, 0.05, self.ramp_length) - - def compute(self, k): - if k < self.initial_delay: - if self.params.movement == "base_circle" or self.params.movement == "walk": - return self.initial_footsteps - - k -= self.initial_delay - out = np.empty((3, 4)) - - if self.params.movement == "base_circle": - out[:] = self._evaluate_circle(k, self.initial_base) - elif self.params.movement == "walk": - out[:] = self.initial_footsteps - else: - out[:] = self.initial_footsteps - if self.params.movement == "circle": - out[:, 1] = self._evaluate_circle(k, self.initial_footsteps[:, 1]) - elif self.params.movement == "step": - out[:, 1] = self._evaluate_step(1, k) - out[2, 1] += 0.015 - else: - out[0, 1] = self.target_ramp_x[k] if k < self.ramp_length else self.target_ramp_x[-1] - out[1, 1] = self.target_ramp_y[k] if k < self.ramp_length else self.target_ramp_y[-1] - out[2, 1] = self.target_ramp_z[k] if k < self.ramp_length else self.target_ramp_z[-1] - - return out - - def _evaluate_circle(self, k, initial_position): - return initial_position + self.offset + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase) - - def _evaluate_step(self, j, k): - n_step = k // self.k_per_step - if n_step % 2 == 0: - return self.p0.copy() if (n_step % 4 == 0) else self.p2.copy() - - if n_step % 4 == 1: - initial = self.p0 - target = self.p2 - velocity = self.v1 - else: - initial = self.p2 - target = self.p0 - velocity = -self.v1 - - k_step = k % self.k_per_step - if n_step != self.update_time: - self.update_interpolator(initial, target, velocity) - self.update_time = n_step - - t = k_step * self.dt_wbc - return self.krog(t) - - def update_interpolator(self, initial, target, velocity): - self.y = [initial, np.zeros(3), self.p1, velocity, target, np.zeros(3)] - self.krog = KroghInterpolator(self.ts, np.array(self.y)) - - -def make_footsteps_and_refs(params, target: Target): - """ - Build a list of both footstep position and base velocity references. - Footsteps is a list of 3,4-matrices - Base_vel_refs is a list of pin.Motion (6D values) - """ - footsteps = [] - base_refs = [] - for k in range(params.N_gait): - target_base_vel = pin.Motion(np.zeros(6)) - kk = k * params.mpc_wbc_ratio - target_footstep = target.compute(kk).copy() - - footsteps.append(target_footstep) - base_refs.append(target_base_vel) - - return footsteps, base_refs diff --git a/python/quadruped_reactive_walking/wb_mpc/task_spec.py b/python/quadruped_reactive_walking/wb_mpc/task_spec.py index 6e9e5b7b..7c00ecec 100644 --- a/python/quadruped_reactive_walking/wb_mpc/task_spec.py +++ b/python/quadruped_reactive_walking/wb_mpc/task_spec.py @@ -78,7 +78,6 @@ def __init__(self, params: Params): self.vertical_velocity_reg_w = task_pms["vertical_velocity_reg_w"] self.base_velocity_tracking_w = task_pms["base_velocity_tracking_w"] - self.foot_tracking_w = task_pms["foot_tracking_w"] self.impact_altitude_w = task_pms["impact_altitude_w"] self.impact_velocity_w = task_pms["impact_velocity_w"] @@ -106,7 +105,6 @@ def __init__(self, params: Params): # Cost function weights self.mu = params.task["mu"] - self.foot_tracking_w = 1e4 self.friction_cone_w = 1e3 self.control_bound_w = 1e3 self.control_reg_w = 1e0 diff --git a/python/quadruped_reactive_walking/wbmpc_wrapper_multiprocess.py b/python/quadruped_reactive_walking/wbmpc_wrapper_multiprocess.py index b8d3ae86..49789289 100644 --- a/python/quadruped_reactive_walking/wbmpc_wrapper_multiprocess.py +++ b/python/quadruped_reactive_walking/wbmpc_wrapper_multiprocess.py @@ -23,12 +23,11 @@ class MultiprocessMPCWrapper(MPCWrapperAbstract): Wrapper to run both types of MPC (OQSP or Crocoddyl) asynchronously in a new process """ - def __init__(self, params: Params, footsteps, base_refs, solver_cls: Type[OCPAbstract]): + def __init__(self, params: Params, base_vel_refs, solver_cls: Type[OCPAbstract]): super().__init__(params) self.solver_cls = solver_cls - self.footsteps_plan = footsteps - self.base_refs = base_refs + self.base_vel_refs = base_vel_refs self.last_available_result: MPCResult = MPCResult(self.N_gait, self.nx, self.nu, self.ndx, self.WINDOW_SIZE) @@ -52,8 +51,7 @@ def __init__(self, params: Params, footsteps, base_refs, solver_cls: Type[OCPAbs self.xs_shared = self.create_shared_ndarray((self.WINDOW_SIZE + 1, self.nx)) self.us_shared = self.create_shared_ndarray((self.WINDOW_SIZE, self.nu)) self.Ks_shared = self.create_shared_ndarray((self.WINDOW_SIZE, self.nu, self.ndx)) - self.footstep_shared = self.create_shared_ndarray((3, 4)) - self.base_ref_shared = self.create_shared_ndarray(6) + self.base_vel_ref_shared = self.create_shared_ndarray(6) self.p = Process(target=self._mpc_asynchronous) self.p.start() @@ -67,8 +65,8 @@ def create_shared_ndarray(self, shape, dtype=np.float64): self._shms.add(shm) return create_shared_ndarray(shape, dtype, shm) - def solve(self, k, x0, footstep, base_vel_ref: pin.Motion): - self._put_shared_data_in(k, x0, footstep, base_vel_ref.np) + def solve(self, k, x0, base_vel_ref: pin.Motion): + self._put_shared_data_in(k, x0, base_vel_ref.np) self.new_data.value = True def get_latest_result(self): @@ -101,8 +99,7 @@ def _mpc_asynchronous(self): """ # Thread-local data x0 = np.zeros_like(self.x0_shared) - footstep = np.zeros_like(self.footstep_shared) - base_ref = np.zeros_like(self.base_ref_shared) + base_vel_ref = np.zeros_like(self.base_vel_ref_shared) while self.running.value: if not self.new_data.value: continue @@ -110,26 +107,25 @@ def _mpc_asynchronous(self): self.new_data.value = False with self.mutex: - k, x0[:], footstep[:], base_ref[:] = self._get_shared_data_in() + k, x0[:], base_vel_ref[:] = self._get_shared_data_in() if k == 0: - loop_ocp = self.solver_cls(self.params, self.footsteps_plan, self.base_refs) + loop_ocp = self.solver_cls(self.params, self.base_vel_refs) - loop_ocp.push_node(k, x0, footstep, base_ref) + loop_ocp.push_node(k, x0, base_vel_ref) loop_ocp.solve(k) gait, xs, us, K, solving_time = loop_ocp.get_results(self.WINDOW_SIZE) self._put_shared_data_out(gait, xs, us, K, loop_ocp.num_iters, solving_time) self.new_result.value = True - def _put_shared_data_in(self, k, x0, footstep, base_ref): + def _put_shared_data_in(self, k, x0, base_vel_ref): """ Put data in shared memory (input to the asynchronous MPC). """ with self.mutex: self.in_k.value = k self.x0_shared[:] = x0 - self.footstep_shared[:] = footstep - self.base_ref_shared[:] = base_ref + self.base_vel_ref_shared[:] = base_vel_ref def _get_shared_data_in(self): """ @@ -137,9 +133,8 @@ def _get_shared_data_in(self): """ k = self.in_k.value x0 = self.x0_shared - footstep = self.footstep_shared - base_ref = self.base_ref_shared - return k, x0, footstep, base_ref + base_vel_ref = self.base_vel_ref_shared + return k, x0, base_vel_ref def _put_shared_data_out(self, gait, xs, us, K, num_iters, solving_time): """Put data in shared memory (output of the asynchronous MPC to be retrieved).""" diff --git a/python/quadruped_reactive_walking/wbmpc_wrapper_ros.py b/python/quadruped_reactive_walking/wbmpc_wrapper_ros.py index 7dcbe4a0..ffe68a35 100644 --- a/python/quadruped_reactive_walking/wbmpc_wrapper_ros.py +++ b/python/quadruped_reactive_walking/wbmpc_wrapper_ros.py @@ -37,8 +37,7 @@ class ROSMPCWrapperClient(MPCWrapperAbstract): def __init__( self, params: Params, - footsteps, - base_refs, + base_vel_refs, solver_cls: Type[OCPAbstract], synchronous=False, ): @@ -51,15 +50,13 @@ def __init__( params.N_gait, self.pd.nx, self.pd.nu, self.pd.ndx, self.WINDOW_SIZE ) - base_refs_multiarray = listof_numpy_to_multiarray_float64(base_refs) - footsteps_multiarray = listof_numpy_to_multiarray_float64(footsteps) + base_vel_refs_multiarray = listof_numpy_to_multiarray_float64(base_vel_refs) init_solver_srv = rospy.ServiceProxy("qrw_wbmpc/init", MPCInit) res = init_solver_srv( solver_type=solver_cls.get_type_str(), params=params.raw_str, - footsteps=footsteps_multiarray, - base_refs=base_refs_multiarray, + base_vel_refs=base_vel_refs_multiarray, ) assert res.success, "Error while initializing mpc on server" @@ -71,12 +68,11 @@ def __init__( "qrw_wbmpc/solve", MPCSolve, callback=self._result_cb, persistent=True ) - def solve(self, k, x0, footstep, base_ref): + def solve(self, k, x0, base_vel_ref): res = self.solve_solver_srv( k=k, x0=numpy_to_multiarray_float64(x0), - footstep=numpy_to_multiarray_float64(footstep), - base_ref=numpy_to_multiarray_float64(base_ref), + base_vel_ref=numpy_to_multiarray_float64(base_vel_ref), ) if self.synchronous: self._parse_result(res) @@ -134,10 +130,9 @@ def _trigger_init(self, msg): self.ndx = self.pd.ndx self.solver_cls = get_ocp_from_str(msg.solver_type) - footsteps = multiarray_to_numpy_float64(msg.footsteps) - base_refs = [pin.Motion(v_ref) for v_ref in multiarray_to_numpy_float64(msg.base_refs)] + base_vel_refs = [pin.Motion(v_ref) for v_ref in multiarray_to_numpy_float64(msg.base_vel_refs)] - self.ocp = self.solver_cls(self.params, footsteps, base_refs) + self.ocp = self.solver_cls(self.params, base_vel_refs) self.last_available_result: MPCResult = MPCResult( self.params.N_gait, self.pd.nx, self.pd.nu, self.pd.ndx, self.WINDOW_SIZE @@ -154,8 +149,7 @@ def _trigger_solve(self, msg): self.ocp.push_node( msg.k, multiarray_to_numpy_float64(msg.x0), - multiarray_to_numpy_float64(msg.footstep), - pin.Motion(multiarray_to_numpy_float64(msg.base_ref)), + pin.Motion(multiarray_to_numpy_float64(msg.base_vel_ref)), ) self.ocp.solve(msg.k) diff --git a/python/quadruped_reactive_walking/wbmpc_wrapper_ros_mp.py b/python/quadruped_reactive_walking/wbmpc_wrapper_ros_mp.py index cebf2afd..0e2e4866 100644 --- a/python/quadruped_reactive_walking/wbmpc_wrapper_ros_mp.py +++ b/python/quadruped_reactive_walking/wbmpc_wrapper_ros_mp.py @@ -15,15 +15,14 @@ class ROSMPCAsyncClient(MultiprocessMPCWrapper): Wrapper to run both types of MPC (OQSP or Crocoddyl) asynchronously in a new process """ - def __init__(self, params: Params, footsteps, base_refs, solver_cls: Type[OCPAbstract]): - self.ros_client = ROSMPCWrapperClient(params, footsteps, base_refs, solver_cls, synchronous=True) + def __init__(self, params: Params, base_vel_refs, solver_cls: Type[OCPAbstract]): + self.ros_client = ROSMPCWrapperClient(params, base_vel_refs, solver_cls, synchronous=True) - super().__init__(params, footsteps, base_refs, solver_cls) + super().__init__(params, base_vel_refs, solver_cls) def _mpc_asynchronous(self): x0 = np.zeros_like(self.x0_shared) - footstep = np.zeros_like(self.footstep_shared) - base_ref = np.zeros_like(self.base_ref_shared) + base_vel_ref = np.zeros_like(self.base_vel_ref_shared) while self.running.value: if not self.new_data.value: continue @@ -31,9 +30,9 @@ def _mpc_asynchronous(self): self.new_data.value = False with self.mutex: - k, x0[:], footstep[:], base_ref[:] = self._get_shared_data_in() + k, x0[:], base_vel_ref[:] = self._get_shared_data_in() - self.ros_client.solve(k, x0, footstep, base_ref) + self.ros_client.solve(k, x0, base_vel_ref) res: MPCResult = self.ros_client.get_latest_result() self._put_shared_data_out(res.gait, res.xs, res.us, res.K, res.num_iters, res.solving_duration) self.new_result.value = True diff --git a/python/quadruped_reactive_walking/wbmpc_wrapper_sync.py b/python/quadruped_reactive_walking/wbmpc_wrapper_sync.py index 9aa74dcb..e4be1950 100644 --- a/python/quadruped_reactive_walking/wbmpc_wrapper_sync.py +++ b/python/quadruped_reactive_walking/wbmpc_wrapper_sync.py @@ -11,19 +11,19 @@ class SyncMPCWrapper(MPCWrapperAbstract): Wrapper to run both types of MPC (OQSP or Crocoddyl) in a synchronous manner in the main thread. """ - def __init__(self, params: Params, footsteps, base_refs, solver_cls: Type[OCPAbstract]): + def __init__(self, params: Params, base_vel_refs, solver_cls: Type[OCPAbstract]): super().__init__(params) self.solver_cls = solver_cls - self.ocp = solver_cls(params, footsteps, base_refs) + self.ocp = solver_cls(params, base_vel_refs) self.last_available_result: MPCResult = MPCResult( self.params.N_gait, self.pd.nx, self.pd.nu, self.pd.ndx, self.WINDOW_SIZE ) self.new_result = False - def solve(self, k, x0, footstep, base_vel_ref): - self.ocp.push_node(k, x0, footstep, base_vel_ref) + def solve(self, k, x0, base_vel_ref): + self.ocp.push_node(k, x0, base_vel_ref) self.ocp.solve(k) gait, xs, us, K, solving_duration = self.ocp.get_results(self.WINDOW_SIZE) diff --git a/ros_qrw_msgs/MPCInit.srv b/ros_qrw_msgs/MPCInit.srv index 715ee0fc..56a8e72a 100644 --- a/ros_qrw_msgs/MPCInit.srv +++ b/ros_qrw_msgs/MPCInit.srv @@ -4,11 +4,8 @@ string solver_type # Parameters of the problem string params -# Gait -std_msgs/Float64MultiArray footsteps - # Base reference speed -std_msgs/Float64MultiArray base_refs +std_msgs/Float64MultiArray base_vel_refs --- diff --git a/ros_qrw_msgs/MPCSolve.srv b/ros_qrw_msgs/MPCSolve.srv index 4a7f9661..6b0e69c9 100644 --- a/ros_qrw_msgs/MPCSolve.srv +++ b/ros_qrw_msgs/MPCSolve.srv @@ -4,11 +4,8 @@ int64 k # Initial conditions std_msgs/Float64MultiArray x0 -# Gait -std_msgs/Float64MultiArray footstep - # Base reference speed -std_msgs/Float64MultiArray base_ref +std_msgs/Float64MultiArray base_vel_ref --- # The solver has been run successfully diff --git a/src/Estimator.cpp b/src/Estimator.cpp index 091e22c1..ceef8b34 100644 --- a/src/Estimator.cpp +++ b/src/Estimator.cpp @@ -28,13 +28,11 @@ Estimator::Estimator() phaseRemainingDuration_(0), feetStancePhaseDuration_(Vector4::Zero()), feetStatus_(Vector4::Zero()), - feetTargets_(Matrix34::Zero()), q_FK_(Vector19::Zero()), v_FK_(Vector18::Zero()), baseVelocityFK_(Vector3::Zero()), basePositionFK_(Vector3::Zero()), b_baseVelocity_(Vector3::Zero()), - feetPositionBarycenter_(Vector3::Zero()), qEstimate_(Vector19::Zero()), vEstimate_(Vector18::Zero()), vSecurity_(Vector12::Zero()), @@ -88,7 +86,6 @@ void Estimator::initialize(Params ¶ms) { } void Estimator::run(MatrixN const &gait, - MatrixN const &feetTargets, VectorN const &baseLinearAcceleration, VectorN const &baseAngularVelocity, VectorN const &baseOrientation, @@ -96,12 +93,11 @@ void Estimator::run(MatrixN const &gait, VectorN const &v, VectorN const &perfectPosition, Vector3 const &b_perfectVelocity) { - updateFeetStatus(gait, feetTargets); + updateFeetStatus(gait); updateIMUData(baseLinearAcceleration, baseAngularVelocity, baseOrientation, perfectPosition); updateJointData(q, v); updateForwardKinematics(); - computeFeetPositionBarycenter(); estimateVelocity(b_perfectVelocity); estimatePosition(perfectPosition.head(3)); @@ -143,9 +139,8 @@ void Estimator::updateReferenceState(VectorN const &vRef) { h_vFiltered_.tail(3) = hRb_ * vFiltered_.tail(3); } -void Estimator::updateFeetStatus(MatrixN const &gait, MatrixN const &feetTargets) { +void Estimator::updateFeetStatus(MatrixN const &gait) { feetStatus_ = gait.row(0); - feetTargets_ = feetTargets; feetStancePhaseDuration_ += feetStatus_; feetStancePhaseDuration_ = feetStancePhaseDuration_.cwiseProduct(feetStatus_); @@ -224,18 +219,6 @@ Vector3 Estimator::computeBasePositionFromFoot(int footId) { return basePosition; } -void Estimator::computeFeetPositionBarycenter() { - int nContactFeet = 0; - Vector3 feetPositions = Vector3::Zero(); - for (int j = 0; j < 4; j++) { - if (feetStatus_(j) == 1.) { - feetPositions += feetTargets_.col(j); - nContactFeet++; - } - } - if (nContactFeet > 0) feetPositionBarycenter_ = feetPositions / nContactFeet; -} - double Estimator::computeAlphaVelocity() { double a = std::ceil(feetStancePhaseDuration_.maxCoeff() * 0.1) - 1; double b = static_cast(phaseRemainingDuration_); @@ -266,7 +249,7 @@ void Estimator::estimateVelocity(Vector3 const &b_perfectVelocity) { void Estimator::estimatePosition(Vector3 const &perfectPosition) { Matrix3 oRb = IMUQuat_.toRotationMatrix(); - Vector3 basePosition = solo3D_ ? perfectPosition : (basePositionFK_ + feetPositionBarycenter_); + Vector3 basePosition = solo3D_ ? perfectPosition : (basePositionFK_); qEstimate_.head(3) = positionFilter_.compute(basePosition, oRb * b_baseVelocity_, alphaPos_); if (perfectEstimator_ || solo3D_) qEstimate_(2) = perfectPosition(2); diff --git a/test_client.py b/test_client.py index 46fad5c1..28a77852 100644 --- a/test_client.py +++ b/test_client.py @@ -41,13 +41,11 @@ def _trigger_solve(self, msg): def fake_in_data(): k = 0 x0 = np.random.randn(37) - footstep = np.random.randn(3, 4) - base_ref = np.array([0.5, 0, 0, 0, 0, 0]) + base_vel_ref = np.array([0.5, 0, 0, 0, 0, 0]) req = MPCSolveRequest( k=k, x0=numpy_to_multiarray_float64(x0), - footstep=numpy_to_multiarray_float64(footstep), - base_ref=numpy_to_multiarray_float64(base_ref), + base_vel_ref=numpy_to_multiarray_float64(base_vel_ref), ) return req From 66887c2406556d821f10fd4e7ac7490975010ef0 Mon Sep 17 00:00:00 2001 From: EtienneAr Date: Wed, 28 Feb 2024 16:28:57 +0100 Subject: [PATCH 2/4] Slightly update ros service test --- test_client.py | 66 ---------------------------- tests/ros/test_ros_service.py | 82 +++++++++++++++++++++++++++++++++++ 2 files changed, 82 insertions(+), 66 deletions(-) delete mode 100644 test_client.py create mode 100644 tests/ros/test_ros_service.py diff --git a/test_client.py b/test_client.py deleted file mode 100644 index 28a77852..00000000 --- a/test_client.py +++ /dev/null @@ -1,66 +0,0 @@ -import rospy -import numpy as np -from quadruped_reactive_walking.srv import ( - MPCSolve, - MPCSolveRequest, - MPCSolveResponse, -) -from quadruped_reactive_walking.tools.ros_tools import ( - numpy_to_multiarray_float64, - listof_numpy_to_multiarray_float64, -) - - -class TestClient: - """ - Wrapper to run both types of MPC (OQSP or Crocoddyl) on a seperate node/machine using ROS as communication interface. - """ - - def __init__( - self, - ): - self.solve_solver_srv = rospy.ServiceProxy("qrw_wbmpc/test", MPCSolve, persistent=True) - - def solve(self): - return self.solve_solver_srv(MPCSolveRequest()) - - -class TestServer: - def __init__(self): - self._solve_service = rospy.Service("qrw_wbmpc/test", MPCSolve, self._trigger_solve) - from quadruped_reactive_walking import MPCResult, Params - - p = Params.create_from_file() - self.res = MPCResult(p.N_gait, 37, 12, 36, p.window_size) - - def _trigger_solve(self, msg): - return fake_out_data(self.res) - # return MPCSolveResponse(run_success=True) - - -def fake_in_data(): - k = 0 - x0 = np.random.randn(37) - base_vel_ref = np.array([0.5, 0, 0, 0, 0, 0]) - req = MPCSolveRequest( - k=k, - x0=numpy_to_multiarray_float64(x0), - base_vel_ref=numpy_to_multiarray_float64(base_vel_ref), - ) - return req - - -def fake_out_data(res): - return MPCSolveResponse( - run_success=True, - gait=numpy_to_multiarray_float64(res.gait), - xs=listof_numpy_to_multiarray_float64(res.xs.tolist()), - us=listof_numpy_to_multiarray_float64(res.us.tolist()), - K=listof_numpy_to_multiarray_float64(res.K.tolist()), - ) - - -if __name__ == "__main__": - rospy.init_node("qrw_wbmpc") - server = TestServer() - rospy.spin() diff --git a/tests/ros/test_ros_service.py b/tests/ros/test_ros_service.py new file mode 100644 index 00000000..231d1bd0 --- /dev/null +++ b/tests/ros/test_ros_service.py @@ -0,0 +1,82 @@ +import argparse +import rospy +import numpy as np +from quadruped_reactive_walking.srv import ( + MPCSolve, + MPCSolveRequest, + MPCSolveResponse, +) +from quadruped_reactive_walking.tools.ros_tools import ( + numpy_to_multiarray_float64, + listof_numpy_to_multiarray_float64, +) + + +class TestClient: + def __init__(self, service_name): + rospy.wait_for_service(service_name, timeout=10) + self.solve_solver_srv = rospy.ServiceProxy(service_name, MPCSolve, persistent=True) + + def fake_request_data(): + k = 0 + x0 = np.random.randn(37) + base_vel_ref = np.array([0.5, 0, 0, 0, 0, 0]) + req = MPCSolveRequest( + k=k, + x0=numpy_to_multiarray_float64(x0), + base_vel_ref=numpy_to_multiarray_float64(base_vel_ref), + ) + return req + + def solve(self): + return self.solve_solver_srv(TestClient.fake_request_data()) + + +class TestServer: + def __init__(self, service_name): + self._solve_service = rospy.Service(service_name, MPCSolve, self._trigger_solve) + from quadruped_reactive_walking import MPCResult, Params + + p = Params.create_from_file() + self.res = MPCResult(p.N_gait, 37, 12, 36, p.window_size) + + def _trigger_solve(self, msg): + return MPCSolveResponse( + run_success=True, + gait=numpy_to_multiarray_float64(self.res.gait), + xs=listof_numpy_to_multiarray_float64(self.res.xs.tolist()), + us=listof_numpy_to_multiarray_float64(self.res.us.tolist()), + K=listof_numpy_to_multiarray_float64(self.res.K.tolist()), + ) + + + +if __name__ == "__main__": + # Parsing arguments + parser = argparse.ArgumentParser() + parser.add_argument("--client", help="Only run client side code", action="store_true") + parser.add_argument("--server", help="Only run server side code", action="store_true") + parser.add_argument("--service", help="Name of the service to test", default="qrw_wbmpc/test") + args = parser.parse_args() + + start_client = (args.client or not args.server) + start_server = (args.server or not args.client) + + # Starintg ros node + rospy.init_node("qrw_wbmpc" + ("_client" if start_client else "") + ("_server" if start_server else "")) + + # Running client and/or server + if(start_server): + server = TestServer(args.service) + + if(start_client): + client = TestClient(args.service) + client.solve() + + # Simple test that everything went fine + assert not rospy.is_shutdown() + + # If the client is not started, let the server running + if(not start_client): + print(f"Server running ({args.service})...") + rospy.spin() \ No newline at end of file From cbe23223c6fb13b1ad8e88a0cc10cc49671b8899 Mon Sep 17 00:00:00 2001 From: EtienneAr Date: Wed, 28 Feb 2024 16:33:32 +0100 Subject: [PATCH 3/4] Remove logging of footstep tracking --- python/quadruped_reactive_walking/tools/logger_control.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/python/quadruped_reactive_walking/tools/logger_control.py b/python/quadruped_reactive_walking/tools/logger_control.py index 75126fca..3f1fb8bf 100644 --- a/python/quadruped_reactive_walking/tools/logger_control.py +++ b/python/quadruped_reactive_walking/tools/logger_control.py @@ -85,7 +85,6 @@ def __init__( self.MPC_equivalent_Kp = np.zeros([size, self.pd.nu]) self.MPC_equivalent_Kd = np.zeros([size, self.pd.nu]) - self.target = np.zeros([size, 3]) self.target_base_linear = np.zeros([size, 3]) self.target_base_angular = np.zeros([size, 3]) @@ -401,7 +400,6 @@ def save(self, filename="data"): np.savez_compressed( name, solver_cls=self.solver_cls, - target=self.target, target_base_linear=self.target_base_linear, target_base_angular=self.target_base_angular, q_estimate_rpy=self.q_estimate_rpy, @@ -455,7 +453,6 @@ def load(self): self.solver_cls = self.data["solver_cls"] # Load sensors arrays - self.target = self.data["target"] self.target_base_linear = self.data["target_base_linear"] self.target_base_angular = self.data["target_base_angular"] self.q_estimate_rpy = self.data["q_estimate_rpy"] From 3a7a6c2601df24c5bed0ed4e74ae2b9f12053e30 Mon Sep 17 00:00:00 2001 From: EtienneAr Date: Wed, 28 Feb 2024 16:41:58 +0100 Subject: [PATCH 4/4] Run pre-commit --- examples/run_jump_ocp.py | 1 + examples/simple_ocp.py | 1 + include/qrw/Estimator.hpp | 14 +++++++------- .../quadruped_reactive_walking/ocp_defs/walking.py | 5 ++--- python/quadruped_reactive_walking/tools/utils.py | 1 + .../wb_mpc/ocp_crocoddyl.py | 1 + tests/ros/test_ros_service.py | 13 ++++++------- 7 files changed, 19 insertions(+), 17 deletions(-) diff --git a/examples/run_jump_ocp.py b/examples/run_jump_ocp.py index af9135de..5dc185a7 100644 --- a/examples/run_jump_ocp.py +++ b/examples/run_jump_ocp.py @@ -2,6 +2,7 @@ import crocoddyl # noqa import aligator import numpy as np +import pinocchio as pin import matplotlib.pyplot as plt from quadruped_reactive_walking import Params diff --git a/examples/simple_ocp.py b/examples/simple_ocp.py index df28081a..6d87eff0 100644 --- a/examples/simple_ocp.py +++ b/examples/simple_ocp.py @@ -1,4 +1,5 @@ import numpy as np +import pinocchio as pin import quadruped_reactive_walking as qrw from quadruped_reactive_walking.controller import Controller import time diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp index ea612ce1..d82ee90b 100644 --- a/include/qrw/Estimator.hpp +++ b/include/qrw/Estimator.hpp @@ -174,13 +174,13 @@ class Estimator { Vector4 feetStancePhaseDuration_; //< Number of loops during which each foot has been in contact Vector4 feetStatus_; //< Contact status of the four feet - pinocchio::Model model_; //< Pinocchio models for frame computations and forward kinematics - pinocchio::Data data_; //< Pinocchio datas for frame computations and forward kinematics - Vector19 q_FK_; //< Configuration vector for Forward Kinematics - Vector18 v_FK_; //< Velocity vector for Forward Kinematics - Vector3 baseVelocityFK_; //< Base linear velocity estimated by Forward Kinematics - Vector3 basePositionFK_; //< Base position estimated by Forward Kinematics - Vector3 b_baseVelocity_; //< Filtered estimated velocity at center base (base frame) + pinocchio::Model model_; //< Pinocchio models for frame computations and forward kinematics + pinocchio::Data data_; //< Pinocchio datas for frame computations and forward kinematics + Vector19 q_FK_; //< Configuration vector for Forward Kinematics + Vector18 v_FK_; //< Velocity vector for Forward Kinematics + Vector3 baseVelocityFK_; //< Base linear velocity estimated by Forward Kinematics + Vector3 basePositionFK_; //< Base position estimated by Forward Kinematics + Vector3 b_baseVelocity_; //< Filtered estimated velocity at center base (base frame) ComplementaryFilter positionFilter_; //< Complementary filter for base position ComplementaryFilter velocityFilter_; //< Complementary filter for base velocity diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py index 8e9c4d63..0b08de20 100644 --- a/python/quadruped_reactive_walking/ocp_defs/walking.py +++ b/python/quadruped_reactive_walking/ocp_defs/walking.py @@ -2,7 +2,7 @@ import pinocchio as pin import crocoddyl -from typing import List, Optional +from typing import Optional from quadruped_reactive_walking import Params, ResidualModelFlyHigh from ..wb_mpc import task_spec from ..tools.utils import no_copy_roll, no_copy_roll_insert @@ -361,7 +361,6 @@ def update_model( self.update_tracking_costs(model.differential.costs, base_vel_ref, support_feet) def update_tracking_costs(self, costs, base_vel_ref: pin.Motion, support_feet): - index = 0 for i in self.task.feet_ids: name = "{}_forceReg".format(self.rmodel.frames[i].name) costs.changeCostStatus(name, i in support_feet) @@ -379,4 +378,4 @@ def update_tracking_costs(self, costs, base_vel_ref: pin.Motion, support_feet): if base_vel_ref is not None and self.has_base_vel_cost: name = "base_velocity_tracking" - costs.costs[name].cost.residual.reference = base_vel_ref \ No newline at end of file + costs.costs[name].cost.residual.reference = base_vel_ref diff --git a/python/quadruped_reactive_walking/tools/utils.py b/python/quadruped_reactive_walking/tools/utils.py index a4b16c2d..043dc505 100644 --- a/python/quadruped_reactive_walking/tools/utils.py +++ b/python/quadruped_reactive_walking/tools/utils.py @@ -7,6 +7,7 @@ except ImportError: from multiprocessing.shared_memory import SharedMemory + def quaternionToRPY(quat): """ Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1) diff --git a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py index 2afdcf7c..43f3590a 100644 --- a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py +++ b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py @@ -10,6 +10,7 @@ from . import task_spec from ..ocp_defs.walking import WalkingOCPBuilder + class CrocOCP(OCPAbstract): """ Generate a Crocoddyl OCP for the control task. diff --git a/tests/ros/test_ros_service.py b/tests/ros/test_ros_service.py index 231d1bd0..52dcfcc7 100644 --- a/tests/ros/test_ros_service.py +++ b/tests/ros/test_ros_service.py @@ -50,7 +50,6 @@ def _trigger_solve(self, msg): ) - if __name__ == "__main__": # Parsing arguments parser = argparse.ArgumentParser() @@ -59,17 +58,17 @@ def _trigger_solve(self, msg): parser.add_argument("--service", help="Name of the service to test", default="qrw_wbmpc/test") args = parser.parse_args() - start_client = (args.client or not args.server) - start_server = (args.server or not args.client) + start_client = args.client or not args.server + start_server = args.server or not args.client # Starintg ros node rospy.init_node("qrw_wbmpc" + ("_client" if start_client else "") + ("_server" if start_server else "")) # Running client and/or server - if(start_server): + if start_server: server = TestServer(args.service) - if(start_client): + if start_client: client = TestClient(args.service) client.solve() @@ -77,6 +76,6 @@ def _trigger_solve(self, msg): assert not rospy.is_shutdown() # If the client is not started, let the server running - if(not start_client): + if not start_client: print(f"Server running ({args.service})...") - rospy.spin() \ No newline at end of file + rospy.spin()