walkgen changes to integrate arm motion

walkgen_footstep_planner/bindings/walkgen.cpp

@@ -216,6 +216,7 @@ void exposeParams() {
       "FootStepPlannerParams",
       bp::init<bp::optional<std::string>>(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,
@@ -296,7 +299,7 @@ void exposeContactSchedule() {
   bp::register_ptr_to_python<std::shared_ptr<ContactSchedule>>();
   bp::class_<ContactSchedule, boost::noncopyable>(
       "ContactSchedule",
-      bp::init<double, int, int, std::vector<std::string>>(bp::args("dt", "T", "S_total", "contactNames"),
+      bp::init<double, int, int, std::vector<std::string>, bool, std::string>(bp::args("dt", "T", "S_total", "contactNames"),
                                                            "Constructor for a ContactSchedule object."))
       .def("addSchedule", &ContactSchedule::addSchedule)
       .def("updateSwitches", &ContactSchedule::updateSwitches)

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

walkgen_footstep_planner/include/ContactSchedule.hpp

@@ -10,7 +10,7 @@
 
 class ContactSchedule {
  public:
-  ContactSchedule(double dt, int T, int S_total, std::vector<std::string> contactNames);
+  ContactSchedule(double dt, int T, int S_total, std::vector<std::string> contactNames, const bool use_arm, std::string arm_name);
   ContactSchedule(const ContactSchedule &other);
   void addSchedule(const std::string &name, const std::vector<std::shared_ptr<ContactPhase>> &schedule);
   void updateSwitches();
@@ -24,6 +24,6 @@ class ContactSchedule {
   std::vector<std::string> contactNames_;                           // contact names
   std::vector<std::vector<std::shared_ptr<ContactPhase>>> phases_;  // phases per each contact
   std::map<int, std::vector<int>> switches_;                        // map of time node to list of contacts
-};
+ };
 
 #endif  // CONTACTSCHEDULE_HPP

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<ContactSchedule> 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<std::string> contactNames_;  // Contact names list.
 
  private:

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<std::string> feet_names;
   std::vector<std::string> feet_names_sl1m;
   std::vector<std::vector<double>> shoulder_offsets;

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],

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
 
 

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"]

walkgen_footstep_planner/src/ContactSchedule.cpp

@@ -1,16 +1,18 @@
 #include "ContactSchedule.hpp"
 #include <stdexcept>
 
-ContactSchedule::ContactSchedule(double dt, int T, int S_total, std::vector<std::string> contactNames)
+ContactSchedule::ContactSchedule(double dt, int T, int S_total, std::vector<std::string> 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)

walkgen_footstep_planner/src/FootStepPlanner.cpp

@@ -134,16 +134,16 @@ MatrixN FootStepPlanner::compute_footstep(std::vector<std::shared_ptr<ContactSch
                                           const VectorN &vq, const VectorN &bvref, const int timeline,
                                           const std::map<std::string, std::vector<Surface>> &selected_surfaces,
                                           const std::map<std::string, Surface> &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<std::shared_ptr<ContactSche
   for (auto cs_iter = queue_cs.rbegin(); cs_iter != queue_cs.rend(); ++cs_iter) {
     auto &cs = **cs_iter;  // Reference to ContactSchedule
     if (cs_index <= horizon_ + 2) {
-      for (size_t c = 0; c < cs.contactNames_.size(); ++c) {
+      size_t exclude_arm = params_.use_arm ? 1 : 0;
+      for (size_t c = 0; c < cs.contactNames_.size()-exclude_arm; ++c) { // removing arm
         auto &name = cs.contactNames_[c];
         size_t j = find_stdVec(cs.contactNames_, name);  // Which foot in foot_timeline
         auto &phases = cs.phases_[c];
@@ -326,16 +327,16 @@ MatrixN FootStepPlanner::update_position(std::vector<std::shared_ptr<ContactSche
 }
 
 void FootStepPlanner::update_current_state(const Eigen::VectorXd &q, const Eigen::VectorXd &vq) {
-  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() != 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)]");
+  // }
   pinocchio::forwardKinematics(model_, data_, q, vq);
   size_t frame_id;
   for (size_t i = 0; i < contactNames_.size(); i++) {

walkgen_footstep_planner/src/Gait.cpp

@@ -1,10 +1,10 @@
 #include <Gait.hpp>
 
-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_);
@@ -40,7 +40,7 @@ std::shared_ptr<ContactSchedule> QuadrupedalGaitGenerator::walk(StdVec_Map_strin
   } else {
     N = N_0 + N_ds + 4 * N_ss - 2 * N_uss - N_uds;
   }
-  std::shared_ptr<ContactSchedule> gait = std::make_shared<ContactSchedule>(dt_, N, S_, contactNames_);
+  std::shared_ptr<ContactSchedule> gait = std::make_shared<ContactSchedule>(dt_, N, S_, contactNames_, use_arm, arm_name);
   std::shared_ptr<FootTrajectoryWrapper> lfSwingTraj =
       std::make_shared<FootTrajectoryWrapper>(dt_, N_ss, stepHeight, contacts[0][lf_], contacts[1][lf_]);
   std::shared_ptr<FootTrajectoryWrapper> lhSwingTraj =
@@ -69,6 +69,16 @@ std::shared_ptr<ContactSchedule> 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<FootTrajectoryWrapper> armTraj =
+      std::make_shared<FootTrajectoryWrapper>(dt_, N, 0, contacts[0][arm_name], contacts[1][arm_name]);  
+      std::vector<std::shared_ptr<ContactPhase>> arm_schedule = {
+      std::make_shared<ContactPhase>(0, ContactType::FULL),
+      std::make_shared<ContactPhase>(N, armTraj),
+      std::make_shared<ContactPhase>(0, ContactType::FULL)};  
+      gait->addSchedule(arm_name, arm_schedule);      
+  }
 
   return gait;
 };
@@ -102,7 +112,7 @@ std::shared_ptr<ContactSchedule> QuadrupedalGaitGenerator::trot(StdVec_Map_strin
   } else {
     N = N_0 + 2 * N_ss - N_uss;
   }
-  std::shared_ptr<ContactSchedule> gait = std::make_shared<ContactSchedule>(dt_, N, S_, contactNames_);
+  std::shared_ptr<ContactSchedule> gait = std::make_shared<ContactSchedule>(dt_, N, S_, contactNames_, use_arm, arm_name);
   std::shared_ptr<FootTrajectoryWrapper> lfSwingTraj =
       std::make_shared<FootTrajectoryWrapper>(dt_, N_ss, stepHeight, contacts[0][lf_], contacts[1][lf_]);
   std::shared_ptr<FootTrajectoryWrapper> lhSwingTraj =
@@ -129,6 +139,16 @@ std::shared_ptr<ContactSchedule> 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<FootTrajectoryWrapper> armTraj =
+      std::make_shared<FootTrajectoryWrapper>(dt_, N, 0, contacts[0][arm_name], contacts[1][arm_name]);  
+      std::vector<std::shared_ptr<ContactPhase>> arm_schedule = {
+      std::make_shared<ContactPhase>(0, ContactType::FULL),
+      std::make_shared<ContactPhase>(N, armTraj),
+      std::make_shared<ContactPhase>(0, ContactType::FULL)};  
+      gait->addSchedule(arm_name, arm_schedule);      
+  }
 
   return gait;
 };

walkgen_footstep_planner/src/GaitManager.cpp

@@ -59,12 +59,19 @@ 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);
+  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);
   } 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 {
@@ -93,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,
@@ -120,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<ContactSchedule> schedule = std::make_shared<ContactSchedule>(*default_schedule_);