From 3c79c61ade12e048530f60f3b77de2eb5a7441e8 Mon Sep 17 00:00:00 2001 From: stonneau Date: Tue, 16 Dec 2025 00:36:05 +0000 Subject: [PATCH 1/4] walkgen changes to integrate arm motion --- walkgen_footstep_planner/bindings/walkgen.cpp | 3 ++ walkgen_footstep_planner/config/params.yaml | 3 +- walkgen_footstep_planner/include/Gait.hpp | 4 ++- walkgen_footstep_planner/include/Params.hpp | 2 ++ .../FootStepManager.py | 22 +++++++++++---- .../walkgen_footstep_planner/GaitManager.py | 24 ++++++++++++++-- .../python/walkgen_footstep_planner/params.py | 10 +++++-- walkgen_footstep_planner/src/Gait.cpp | 28 +++++++++++++++++-- walkgen_footstep_planner/src/GaitManager.cpp | 2 +- walkgen_footstep_planner/src/Params.cpp | 11 ++++++++ 10 files changed, 91 insertions(+), 18 deletions(-) diff --git a/walkgen_footstep_planner/bindings/walkgen.cpp b/walkgen_footstep_planner/bindings/walkgen.cpp index 75242a1..03b585d 100644 --- a/walkgen_footstep_planner/bindings/walkgen.cpp +++ b/walkgen_footstep_planner/bindings/walkgen.cpp @@ -216,6 +216,7 @@ void exposeParams() { "FootStepPlannerParams", bp::init>(bp::args("filename"), "Constructor for parameter to laod the yaml.")) .def_readwrite("typeGait", &Params::type, "Type of gait") + .def_readwrite("arm_name", &Params::arm_name, "name of the arm") .def_readwrite("dt", &Params::dt, "Time step duration") .def_readwrite("horizon", &Params::horizon, "Planning horizon (in steps)") .def_readwrite("nsteps", &Params::nsteps, "Number of steps to plan") @@ -234,6 +235,8 @@ void exposeParams() { .def_readwrite("trot_N_uds", &Params::trot_N_uds, "Trotting Number of unloading double support phases") .def_readwrite("trot_N_uss", &Params::trot_N_uss, "Trotting Number of unloading single support phases") + .def_readwrite("use_arm", &Params::use_arm, + "If True, a reference trajectory for the robot gripper is generated") .def_readwrite("reactive_planning", &Params::reactive_planning, "Set to True to use low-pass filter on the base or False for rigid estimation.") .def_readwrite("N_phase_return", &Params::N_phase_return, diff --git a/walkgen_footstep_planner/config/params.yaml b/walkgen_footstep_planner/config/params.yaml index dc88477..9b8be5e 100644 --- a/walkgen_footstep_planner/config/params.yaml +++ b/walkgen_footstep_planner/config/params.yaml @@ -4,7 +4,8 @@ walkgen_params: # Use reactive planning (base filtered with a low-pass filter) by setting to True, # or use rigid planning (base estimator relies only on the commanded velocity) : reactive_planning: true - + use_arm: true # if true contacts will be created for the arm + arm_name: "gripperMover" gait: type: "trot" # Only "walk" or "trot" working dt: 0.01 diff --git a/walkgen_footstep_planner/include/Gait.hpp b/walkgen_footstep_planner/include/Gait.hpp index c91e274..7276d0a 100644 --- a/walkgen_footstep_planner/include/Gait.hpp +++ b/walkgen_footstep_planner/include/Gait.hpp @@ -19,7 +19,7 @@ class QuadrupedalGaitGenerator { // Constructor QuadrupedalGaitGenerator(double dt = 0.01, int S = 4, std::string lf = "LF_FOOT", std::string lh = "LH_FOOT", - std::string rf = "RF_FOOT", std::string rh = "RH_FOOT"); + std::string rf = "RF_FOOT", std::string rh = "RH_FOOT", std::string arm_name = "gripperMover", const bool use_arm=false); std::shared_ptr walk(StdVec_Map_string_SE3 contacts, int N_ds, int N_ss, int N_uds, int N_uss, double stepHeight, bool startPhase, bool endPhase); @@ -33,6 +33,8 @@ class QuadrupedalGaitGenerator { std::string lh_; // Left hind name std::string rf_; // Right front name std::string rh_; // Right hind name + const std::string arm_name; // name of the arm + const bool use_arm; // whether to plan the arm trajectory std::vector contactNames_; // Contact names list. private: diff --git a/walkgen_footstep_planner/include/Params.hpp b/walkgen_footstep_planner/include/Params.hpp index ce93aa1..f0f5b19 100644 --- a/walkgen_footstep_planner/include/Params.hpp +++ b/walkgen_footstep_planner/include/Params.hpp @@ -29,6 +29,7 @@ class Params { // Gait parameters std::string type; + std::string arm_name; double dt; int horizon; int nsteps; @@ -39,6 +40,7 @@ class Params { int N_uss; int N_phase_return; bool reactive_planning; // Set to True to use low-pass filter on the base or False for rigid estimation. + bool use_arm; // If True, a reference trajectory for the robot gripper is generated std::vector feet_names; std::vector feet_names_sl1m; std::vector> shoulder_offsets; diff --git a/walkgen_footstep_planner/python/walkgen_footstep_planner/FootStepManager.py b/walkgen_footstep_planner/python/walkgen_footstep_planner/FootStepManager.py index 91871f8..21cf847 100644 --- a/walkgen_footstep_planner/python/walkgen_footstep_planner/FootStepManager.py +++ b/walkgen_footstep_planner/python/walkgen_footstep_planner/FootStepManager.py @@ -138,12 +138,22 @@ def initialize_default_cs(self): """ Create a default contact schedule compatible with Caracal CS. contacts, N_ds, N_ss, N_uss=0, N_uds=0, stepHeight=0.15, startPhase=True, endPhase=True """ - gait_generator = QuadrupedalGaitGenerator(dt=self._dt, - S=4, - lf=self._contactNames[0], - lh=self._contactNames[1], - rf=self._contactNames[2], - rh=self._contactNames[3]) + arm_name = None + gait_generator = None + if (self._params.use_arm): + gait_generator = QuadrupedalGaitGenerator(dt=self._dt, + S=4, + lf=self._contactNames[0], + lh=self._contactNames[1], + rf=self._contactNames[2], + rh=self._contactNames[3], frame_names=[self._params.arm_name]) + else: + gait_generator = QuadrupedalGaitGenerator(dt=self._dt, + S=4, + lf=self._contactNames[0], + lh=self._contactNames[1], + rf=self._contactNames[2], + rh=self._contactNames[3]) if self._typeGait == "trot": self._initial_cs = copy.deepcopy( gait_generator.trot(contacts=[self._gait_manager.cs0, self._gait_manager.cs1], diff --git a/walkgen_footstep_planner/python/walkgen_footstep_planner/GaitManager.py b/walkgen_footstep_planner/python/walkgen_footstep_planner/GaitManager.py index bc2756d..bd1daf2 100644 --- a/walkgen_footstep_planner/python/walkgen_footstep_planner/GaitManager.py +++ b/walkgen_footstep_planner/python/walkgen_footstep_planner/GaitManager.py @@ -448,11 +448,14 @@ class QuadrupedalGaitGenerator: """ Create quadrupedal gait with polynomial swing foot trajectory. It uses a custom Trajectory object, independant from Caracal. """ - def __init__(self, dt=1e-2, S=4, lf="LF_FOOT", lh="LH_FOOT", rf="RF_FOOT", rh="RH_FOOT"): + def __init__(self, dt=1e-2, S=4, lf="LF_FOOT", lh="LH_FOOT", rf="RF_FOOT", rh="RH_FOOT", arm_name=None): self._dt = dt self._S = S self._contactNames = [lf, lh, rf, rh] - + self.use_arm = arm_name is not None + self.arm_name = arm_name + + def walk(self, contacts, N_ds, N_ss, N_uds=0, N_uss=0, stepHeight=0.15, startPhase=True, endPhase=True): N_0 = 0 if startPhase: @@ -485,7 +488,16 @@ def walk(self, contacts, N_ds, N_ss, N_uds=0, N_uss=0, stepHeight=0.15, startPha ContactPhase(N_0 + N_ds + 3 * N_ss - 2 * N_uss - N_uds), ContactPhase(N_ss, trajectory=rfSwingTraj), ContactPhase(N - (N_0 + N_ds + 4 * N_ss - 2 * N_uss - N_uds)) - ]) + ]) + + if (self.use_arm): + armtraj = FootStepTrajectoryBezier(self._dt, N_ss, 0., contacts[0][rf], contacts[1][rf]) #TODO Z + gait.addSchedule(rf, [ + ContactPhase(0, contactType=caracal.ContactType.FULL), + ContactPhase(N_ss, trajectory=armtraj), + ContactPhase(0, contactType=caracal.ContactType.FULL) + ]) + return gait def trot(self, contacts, N_ds, N_ss, N_uss=0, N_uds=0, stepHeight=0.15, startPhase=True, endPhase=True): @@ -521,6 +533,12 @@ def trot(self, contacts, N_ds, N_ss, N_uss=0, N_uds=0, stepHeight=0.15, startPha ContactPhase(N - (N_0 + 2 * N_ss - N_uss)) ]) + if (self.use_arm): + armtraj = FootStepTrajectoryBezier(self._dt, N_ss, 0., contacts[0][rf], contacts[1][rf]) #TODO Z + gait.addSchedule(rf, [ + ContactPhase(N_0 + N_ds + 3 * N_ss - 2 * N_uss - N_uds), + ContactPhase(N_ss, trajectory=rfSwingTraj), + ContactPhase(N - (N_0 + N_ds + 4 * N_ss - 2 * N_uss - N_uds)) return gait diff --git a/walkgen_footstep_planner/python/walkgen_footstep_planner/params.py b/walkgen_footstep_planner/python/walkgen_footstep_planner/params.py index 6ea8d7b..f7d52f3 100644 --- a/walkgen_footstep_planner/python/walkgen_footstep_planner/params.py +++ b/walkgen_footstep_planner/python/walkgen_footstep_planner/params.py @@ -59,6 +59,8 @@ def __init__(self, filename=None): self.feet_names = ["FL_foot", "RL_foot", "FR_foot", "RR_foot"] # b1 names self.feet_names_sl1m = ["FL_foot", "RL_foot", "FR_foot", "RR_foot"] # b1 names self.shoulder_offsets = [[0.367, 0.2], [0.367, -0.2], [-0.367, 0.2], [-0.367, -0.2]] + self.arm_name = "gripperMover" + self.use_arm = False # self.feet_order = [0,1,2,3] # [Left Front ; Left Hind ; Right Front ; Right Hind] # Bezier parameters @@ -84,6 +86,8 @@ def parseFile(self, filename): """ config = yaml.load(open(filename, 'r'), Loader=yaml.FullLoader) self.N_phase_return = config["walkgen_params"]["params"]["N_phase_return"] + self.use_arm = config["walkgen_params"]["use_arm"] + self.arm_name = config["walkgen_params"]["params"]["arm_name"] self.typeGait = config["walkgen_params"]["gait"]["type"] self.dt = config["walkgen_params"]["gait"]["dt"] self.N_ds = config["walkgen_params"]["gait"][self.typeGait]["N_ds"] @@ -93,9 +97,9 @@ def parseFile(self, filename): self.horizon = config["walkgen_params"]["gait"]["horizon"] self.nsteps = config["walkgen_params"]["gait"]["nsteps"] self.stepHeight = config["walkgen_params"]["gait"]["stepHeight"] - feet_names = config["walkgen_params"]["gait"]["feet_names"] - feet_names_sl1m = config["walkgen_params"]["gait"]["feet_names_sl1m"] - shoulder_offsets = config["walkgen_params"]["gait"]["shoulder_offsets"] + self.feet_names = config["walkgen_params"]["gait"]["feet_names"] + self.feet_names_sl1m = config["walkgen_params"]["gait"]["feet_names_sl1m"] + self.shoulder_offsets = config["walkgen_params"]["gait"]["shoulder_offsets"] self.margin = config["walkgen_params"]["bezier"]["margin"] self.t_margin = config["walkgen_params"]["bezier"]["t_margin"] self.z_margin = config["walkgen_params"]["bezier"]["z_margin"] diff --git a/walkgen_footstep_planner/src/Gait.cpp b/walkgen_footstep_planner/src/Gait.cpp index 5545414..94686b3 100644 --- a/walkgen_footstep_planner/src/Gait.cpp +++ b/walkgen_footstep_planner/src/Gait.cpp @@ -1,10 +1,10 @@ #include -QuadrupedalGaitGenerator::~QuadrupedalGaitGenerator(){}; +QuadrupedalGaitGenerator::~QuadrupedalGaitGenerator(){} QuadrupedalGaitGenerator::QuadrupedalGaitGenerator(double dt, int S, std::string lf, std::string lh, std::string rf, - std::string rh) - : dt_(dt), S_(S), lf_(lf), lh_(lh), rf_(rf), rh_(rh) { + std::string rh, std::string arm_name, const bool use_arm) + : dt_(dt), S_(S), lf_(lf), lh_(lh), rf_(rf), rh_(rh), arm_name(arm_name), use_arm(use_arm) { contactNames_.push_back(lf_); contactNames_.push_back(lh_); contactNames_.push_back(rf_); @@ -69,6 +69,16 @@ std::shared_ptr QuadrupedalGaitGenerator::walk(StdVec_Map_strin gait->addSchedule(lf_, lf_schedule); gait->addSchedule(rh_, rh_schedule); gait->addSchedule(rf_, rf_schedule); + if(use_arm) + { + std::shared_ptr armTraj = + std::make_shared(dt_, N, 0, contacts[0][rf_], contacts[1][rf_]); // aligned on right foot + std::vector> arm_schedule = { + std::make_shared(0, ContactType::FULL), + std::make_shared(N, armTraj), + std::make_shared(0, ContactType::FULL)}; + gait->addSchedule(rf_, arm_schedule); + } return gait; }; @@ -129,6 +139,18 @@ std::shared_ptr QuadrupedalGaitGenerator::trot(StdVec_Map_strin gait->addSchedule(rf_, rf_schedule); gait->addSchedule(lf_, lf_schedule); gait->addSchedule(rh_, rh_schedule); + if(use_arm) + { + std::shared_ptr armTraj = + std::make_shared(dt_, N, 0, contacts[0][rf_], contacts[1][rf_]); // aligned on right foot + std::vector> arm_schedule = { + std::make_shared(0, ContactType::FULL), + std::make_shared(N, armTraj), + std::make_shared(0, ContactType::FULL)}; + gait->addSchedule(rf_, arm_schedule); + } + + return gait; return gait; }; diff --git a/walkgen_footstep_planner/src/GaitManager.cpp b/walkgen_footstep_planner/src/GaitManager.cpp index cfecfe0..e3c05d5 100644 --- a/walkgen_footstep_planner/src/GaitManager.cpp +++ b/walkgen_footstep_planner/src/GaitManager.cpp @@ -60,7 +60,7 @@ void GaitManager::initialize(const pinocchio::Model &model, const VectorN &q) { cs1[rh] = data.oMf[model.getFrameId(rh)]; cs1[rf] = data.oMf[model.getFrameId(rf)]; - QuadrupedalGaitGenerator gait_generator_ = QuadrupedalGaitGenerator(dt_, 4, lf, lh, rf, rh); + QuadrupedalGaitGenerator gait_generator_ = QuadrupedalGaitGenerator(dt_, 4, lf, lh, rf, rh, params_.arm_name, params_.use_arm); if (type_ == "trot") { initial_schedule_ = gait_generator_.trot({cs0, cs1}, 50, N_ss_, N_uss_, N_uds_, stepHeight_, true, false); default_schedule_ = gait_generator_.trot({cs0, cs1}, N_ds_, N_ss_, N_uss_, N_uds_, stepHeight_, false, false); diff --git a/walkgen_footstep_planner/src/Params.cpp b/walkgen_footstep_planner/src/Params.cpp index c223547..653e644 100644 --- a/walkgen_footstep_planner/src/Params.cpp +++ b/walkgen_footstep_planner/src/Params.cpp @@ -30,6 +30,8 @@ void Params::initialize_default() { shoulder_offsets.push_back({0.367, -0.2}); shoulder_offsets.push_back({-0.367, 0.2}); shoulder_offsets.push_back({-0.367, -0.2}); + arm_name = "None"; + use_arm = false; walk_N_ds = 90; walk_N_ss = 70; @@ -76,6 +78,8 @@ Params::Params(const Params &other) { this->N_sample_ineq = other.N_sample_ineq; this->degree = other.degree; this->early_termination_ratio = other.early_termination_ratio; + this->arm_name = other.arm_name; + this->use_arm = other.use_arm; // Gait parametres for changing this->walk_N_ds = other.walk_N_ds; @@ -100,6 +104,13 @@ void Params::parse_yaml_file(const std::string &filename) { assert_yaml_parsing(config["params"], filename, "reactive_planning"); reactive_planning = config["params"]["reactive_planning"].as(); + + assert_yaml_parsing(config["params"], filename, "use_arm"); + use_arm = config["params"]["use_arm"].as(); + + + assert_yaml_parsing(config["params"], filename, "arm_name"); + arm_name = config["params"]["arm_name"].as(); // assert_yaml_parsing(config, filename, "gait"); assert_yaml_parsing(config["gait"], filename, "type"); From 9332bedc95b98b86eae756170bd5829047c9e00e Mon Sep 17 00:00:00 2001 From: stonneau Date: Tue, 16 Dec 2025 01:02:30 +0000 Subject: [PATCH 2/4] adding initial position of arm to contact plan in planner initialisation --- walkgen_footstep_planner/src/Gait.cpp | 2 -- walkgen_footstep_planner/src/GaitManager.cpp | 6 ++++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/walkgen_footstep_planner/src/Gait.cpp b/walkgen_footstep_planner/src/Gait.cpp index 94686b3..5dc6ac0 100644 --- a/walkgen_footstep_planner/src/Gait.cpp +++ b/walkgen_footstep_planner/src/Gait.cpp @@ -151,6 +151,4 @@ std::shared_ptr QuadrupedalGaitGenerator::trot(StdVec_Map_strin } return gait; - - return gait; }; diff --git a/walkgen_footstep_planner/src/GaitManager.cpp b/walkgen_footstep_planner/src/GaitManager.cpp index e3c05d5..740ac6f 100644 --- a/walkgen_footstep_planner/src/GaitManager.cpp +++ b/walkgen_footstep_planner/src/GaitManager.cpp @@ -59,6 +59,12 @@ void GaitManager::initialize(const pinocchio::Model &model, const VectorN &q) { cs1[lf] = data.oMf[model.getFrameId(lf)]; cs1[rh] = data.oMf[model.getFrameId(rh)]; cs1[rf] = data.oMf[model.getFrameId(rf)]; + + if(params_.use_arm) + { + cs0[params_.arm_name] = data.oMf[model.getFrameId(params_.arm_name)]; + cs1[params_.arm_name] = data.oMf[model.getFrameId(params_.arm_name)]; + } QuadrupedalGaitGenerator gait_generator_ = QuadrupedalGaitGenerator(dt_, 4, lf, lh, rf, rh, params_.arm_name, params_.use_arm); if (type_ == "trot") { From 2ea4f2dafd12f72ed3a27ea7dfb793b4fed2f6f0 Mon Sep 17 00:00:00 2001 From: Sergim96 Date: Tue, 16 Dec 2025 15:57:18 +0000 Subject: [PATCH 3/4] propagated arm trajectory handling to contact phase, controller now working to mainting arm in position --- walkgen_footstep_planner/bindings/walkgen.cpp | 2 +- .../include/ContactSchedule.hpp | 4 +- .../src/ContactSchedule.cpp | 6 ++- .../src/FootStepPlanner.cpp | 43 ++++++++++--------- walkgen_footstep_planner/src/Gait.cpp | 16 +++---- walkgen_footstep_planner/src/GaitManager.cpp | 3 +- 6 files changed, 39 insertions(+), 35 deletions(-) diff --git a/walkgen_footstep_planner/bindings/walkgen.cpp b/walkgen_footstep_planner/bindings/walkgen.cpp index 03b585d..8d53405 100644 --- a/walkgen_footstep_planner/bindings/walkgen.cpp +++ b/walkgen_footstep_planner/bindings/walkgen.cpp @@ -299,7 +299,7 @@ void exposeContactSchedule() { bp::register_ptr_to_python>(); bp::class_( "ContactSchedule", - bp::init>(bp::args("dt", "T", "S_total", "contactNames"), + bp::init, bool, std::string>(bp::args("dt", "T", "S_total", "contactNames"), "Constructor for a ContactSchedule object.")) .def("addSchedule", &ContactSchedule::addSchedule) .def("updateSwitches", &ContactSchedule::updateSwitches) diff --git a/walkgen_footstep_planner/include/ContactSchedule.hpp b/walkgen_footstep_planner/include/ContactSchedule.hpp index 8f38bbe..a84a279 100644 --- a/walkgen_footstep_planner/include/ContactSchedule.hpp +++ b/walkgen_footstep_planner/include/ContactSchedule.hpp @@ -10,7 +10,7 @@ class ContactSchedule { public: - ContactSchedule(double dt, int T, int S_total, std::vector contactNames); + ContactSchedule(double dt, int T, int S_total, std::vector contactNames, const bool use_arm, std::string arm_name); ContactSchedule(const ContactSchedule &other); void addSchedule(const std::string &name, const std::vector> &schedule); void updateSwitches(); @@ -24,6 +24,6 @@ class ContactSchedule { std::vector contactNames_; // contact names std::vector>> phases_; // phases per each contact std::map> switches_; // map of time node to list of contacts -}; + }; #endif // CONTACTSCHEDULE_HPP diff --git a/walkgen_footstep_planner/src/ContactSchedule.cpp b/walkgen_footstep_planner/src/ContactSchedule.cpp index 632b394..8142cc5 100644 --- a/walkgen_footstep_planner/src/ContactSchedule.cpp +++ b/walkgen_footstep_planner/src/ContactSchedule.cpp @@ -1,16 +1,18 @@ #include "ContactSchedule.hpp" #include -ContactSchedule::ContactSchedule(double dt, int T, int S_total, std::vector contactNames) +ContactSchedule::ContactSchedule(double dt, int T, int S_total, std::vector contactNames, const bool use_arm, std::string arm_name) : dt_(dt), T_(T), S_total_(S_total), C_(0), contactNames_(std::move(contactNames)), - phases_(contactNames_.size()), switches_() { + if(use_arm) + contactNames_.push_back(arm_name); std::sort(contactNames_.begin(), contactNames_.end()); C_ = int(contactNames_.size()); + phases_.resize(C_); } ContactSchedule::ContactSchedule(const ContactSchedule &other) diff --git a/walkgen_footstep_planner/src/FootStepPlanner.cpp b/walkgen_footstep_planner/src/FootStepPlanner.cpp index 81d98c3..b236cda 100644 --- a/walkgen_footstep_planner/src/FootStepPlanner.cpp +++ b/walkgen_footstep_planner/src/FootStepPlanner.cpp @@ -134,16 +134,16 @@ MatrixN FootStepPlanner::compute_footstep(std::vector> &selected_surfaces, const std::map &previous_surfaces) { - if (q.size() != 19) { - throw std::runtime_error( - "Current state q should be an array of size 19 " - "[pos (x3), quaternion (x4), joint (x12)]"); - } - if (vq.size() != 18) { - throw std::runtime_error( - "Current velocity vq should be an array of size " - "18 [lin vel (x3), ang vel (x3), joint vel(x12)]"); - } + // if (q.size() != model_) { + // throw std::runtime_error( + // "Current state q should be an array of size 19 " + // "[pos (x3), quaternion (x4), joint (x12)]"); + // } + // if (vq.size() != 18) { + // throw std::runtime_error( + // "Current velocity vq should be an array of size " + // "18 [lin vel (x3), ang vel (x3), joint vel(x12)]"); + // } if (bvref.size() != 6) { throw std::runtime_error( "Reference velocity should be an array of size 6 " @@ -199,7 +199,8 @@ MatrixN FootStepPlanner::update_position(std::vector QuadrupedalGaitGenerator::walk(StdVec_Map_strin } else { N = N_0 + N_ds + 4 * N_ss - 2 * N_uss - N_uds; } - std::shared_ptr gait = std::make_shared(dt_, N, S_, contactNames_); + std::shared_ptr gait = std::make_shared(dt_, N, S_, contactNames_, use_arm, arm_name); std::shared_ptr lfSwingTraj = std::make_shared(dt_, N_ss, stepHeight, contacts[0][lf_], contacts[1][lf_]); std::shared_ptr lhSwingTraj = @@ -72,12 +72,12 @@ std::shared_ptr QuadrupedalGaitGenerator::walk(StdVec_Map_strin if(use_arm) { std::shared_ptr armTraj = - std::make_shared(dt_, N, 0, contacts[0][rf_], contacts[1][rf_]); // aligned on right foot + std::make_shared(dt_, N, 0, contacts[0][arm_name], contacts[1][arm_name]); std::vector> arm_schedule = { std::make_shared(0, ContactType::FULL), std::make_shared(N, armTraj), - std::make_shared(0, ContactType::FULL)}; - gait->addSchedule(rf_, arm_schedule); + std::make_shared(0, ContactType::FULL)}; + gait->addSchedule(arm_name, arm_schedule); } return gait; @@ -112,7 +112,7 @@ std::shared_ptr QuadrupedalGaitGenerator::trot(StdVec_Map_strin } else { N = N_0 + 2 * N_ss - N_uss; } - std::shared_ptr gait = std::make_shared(dt_, N, S_, contactNames_); + std::shared_ptr gait = std::make_shared(dt_, N, S_, contactNames_, use_arm, arm_name); std::shared_ptr lfSwingTraj = std::make_shared(dt_, N_ss, stepHeight, contacts[0][lf_], contacts[1][lf_]); std::shared_ptr lhSwingTraj = @@ -142,12 +142,12 @@ std::shared_ptr QuadrupedalGaitGenerator::trot(StdVec_Map_strin if(use_arm) { std::shared_ptr armTraj = - std::make_shared(dt_, N, 0, contacts[0][rf_], contacts[1][rf_]); // aligned on right foot + std::make_shared(dt_, N, 0, contacts[0][arm_name], contacts[1][arm_name]); std::vector> arm_schedule = { std::make_shared(0, ContactType::FULL), std::make_shared(N, armTraj), - std::make_shared(0, ContactType::FULL)}; - gait->addSchedule(rf_, arm_schedule); + std::make_shared(0, ContactType::FULL)}; + gait->addSchedule(arm_name, arm_schedule); } return gait; diff --git a/walkgen_footstep_planner/src/GaitManager.cpp b/walkgen_footstep_planner/src/GaitManager.cpp index 740ac6f..7f44dd3 100644 --- a/walkgen_footstep_planner/src/GaitManager.cpp +++ b/walkgen_footstep_planner/src/GaitManager.cpp @@ -71,6 +71,7 @@ void GaitManager::initialize(const pinocchio::Model &model, const VectorN &q) { initial_schedule_ = gait_generator_.trot({cs0, cs1}, 50, N_ss_, N_uss_, N_uds_, stepHeight_, true, false); default_schedule_ = gait_generator_.trot({cs0, cs1}, N_ds_, N_ss_, N_uss_, N_uds_, stepHeight_, false, false); } else if (type_ == "walk") { + initial_schedule_ = gait_generator_.walk({cs0, cs1}, 100, N_ss_, N_uss_, N_uds_, stepHeight_, true, false); default_schedule_ = gait_generator_.walk({cs0, cs1}, N_ds_, N_ss_, N_uss_, N_uds_, stepHeight_, false, false); } else { @@ -99,6 +100,7 @@ void GaitManager::initialize(const pinocchio::Model &model, const VectorN &q) { // Next MPC gait cmd_gait_ = 0; // [0 --> Default, 1 --> Walk, 2 --> Trot] // Register the 2 type of shcedule + trot_schedule_ = gait_generator_.trot({cs0, cs1}, params_.trot_N_ds, params_.trot_N_ss, params_.trot_N_uss, params_.trot_N_uds, stepHeight_, false, false); walk_schedule_ = gait_generator_.walk({cs0, cs1}, params_.walk_N_ds, params_.walk_N_ss, params_.walk_N_uss, @@ -126,7 +128,6 @@ void GaitManager::initialize(const pinocchio::Model &model, const VectorN &q) { queue_cs_.push_back(initial_schedule_); // Add default CS depending on the horizon length. - std::cout << "gait manager horizon : " << horizon_ << std::endl; double T_global = initial_schedule_->T_; while (T_global < horizon_) { std::shared_ptr schedule = std::make_shared(*default_schedule_); From b60625896db1e84a285254ac49b812fe703f07e2 Mon Sep 17 00:00:00 2001 From: Sergim96 Date: Tue, 16 Dec 2025 16:04:18 +0000 Subject: [PATCH 4/4] tune planner --- .../walkgen_surface_planner/SurfacePlanner.py | 4 +-- .../tools/geometry_utils.py | 26 +++++++++++++++++-- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py b/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py index 8456b98..cf9537a 100644 --- a/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py +++ b/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py @@ -562,9 +562,9 @@ def run(self, q, gait_in, timings_in, bvref, target_foostep): feet_selected = [2, 3] if pitch > 0.05: feet_selected = [0, 1] # Going down forward - costs["effector_positions_3D_select"] = [0.25, [feet_selected, shoulder_position]] + costs["effector_positions_3D_select"] = [0., [feet_selected, shoulder_position]] else: - costs["effector_positions_3D_select"] = [0.2, [feet_selected, shoulder_position]] + costs["effector_positions_3D_select"] = [0., [feet_selected, shoulder_position]] ############################################################# ############################################################# diff --git a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py index b1b698c..b027979 100644 --- a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py +++ b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py @@ -425,6 +425,28 @@ def order(points): output.pop(0) return [points[int(elt)].tolist() for elt in output] +import visvalingamwyatt as vw +def remove_duplicates(points, threshold=0.001): -def remove_duplicates(points): - return np.unique(points, axis=0) + """ Decimate the number of point using visvalingamwyatt algorithm. + """ + # points = [[point.x, point.y] for point in hole.points ] + simplifier = vw.Simplifier(points) + points_filtered = simplifier.simplify(threshold=threshold) + return np.array(points_filtered) + +# ~ def remove_duplicates(points, threshold=0.02): + # ~ filtered = [] + # ~ points = np.asarray(points) + + # ~ for p in points: + # ~ if len(filtered) == 0: + # ~ filtered.append(p) + # ~ continue + + # ~ # distance to all previously kept points + # ~ d = np.linalg.norm(np.array(filtered) - p, axis=1) + + # ~ if np.all(d > threshold): + # ~ filtered.append(p) + # ~ return np.array(filtered)