Estimation of the feet by FK in Bezier planner, in cpp, removed from controller

include/qrw/FootTrajectoryGeneratorBezier.hpp

@@ -15,6 +15,13 @@
 #include "qrw/Types.h"
 #include "qrw/Params.hpp"
 
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
 #include "ndcurves/fwd.h"
 #include "ndcurves/bezier_curve.h"
 #include "ndcurves/optimization/details.h"
@@ -60,8 +67,9 @@ class FootTrajectoryGeneratorBezier {
   ///
   /// \brief updates the nex foot position, velocity and acceleration, and the foot goal position
   ///
-  /// \param[in] j foot id
-  /// \param[in] targetFootstep desired target location at the end of the swing phase
+  /// \param[in] k (int): number of time steps since the start of the simulation
+  /// \param[in] j (int): index of the foot
+  /// \param[in] targetFootstep (Vector3): desired target location at the end of the swing phase
   ///
   ////////////////////////////////////////////////////////////////////////////////////////////////
   void updateFootPosition(int const& k, int const& i_foot, Vector3 const& targetFootstep);
@@ -72,10 +80,21 @@ class FootTrajectoryGeneratorBezier {
   ///        to the desired position on the ground (computed by the footstep planner)
   ///
   /// \param[in] k (int): number of time steps since the start of the simulation
+  /// \param[in] surfacesSelected (SurfaceVector): Vector of contact surfaces for each foot
+  /// \param[in] targetFootstep (Matrix34): desired target location at the end of the swing phase
+  /// \param[in] q (Vector18): State of the robot, (RPY formulation, size 18)
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void update(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected, VectorN const& q);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Update the 3D position of the feet in world frame by forward kinematic, matrix position_FK_
+  ///
+  /// \param[in] q (Vector18): State of the robot, (RPY formulation, size 18)
   ///
   ////////////////////////////////////////////////////////////////////////////////////////////////
-  void update(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected,
-              MatrixN const& currentPosition);
+  void update_position_FK(VectorN const& q);
 
   void updatePolyCoeff_XY(int const& i_foot, Vector3 const& x_init, Vector3 const& v_init, Vector3 const& a_init,
                           Vector3 const& x_target, double const& t0, double const& t1);
@@ -120,6 +139,7 @@ class FootTrajectoryGeneratorBezier {
   Matrix74 Az;  ///< Coefficients for the Z component
 
   Matrix34 position_;      // position computed in updateFootPosition
+  Matrix34 position_FK_;   // position computed by Forward dynamics
   Matrix34 velocity_;      // velocity computed in updateFootPosition
   Matrix34 acceleration_;  // acceleration computed in updateFootPosition
   Matrix34 jerk_;          // Jerk computed in updateFootPosition
@@ -176,9 +196,15 @@ class FootTrajectoryGeneratorBezier {
   // QP solver
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
   EiquadprogFast_status status;
-
   EiquadprogFast qp;
 
+  // Pinocchio model for foot estimation
+  pinocchio::Model model_;          // Pinocchio model for forward kinematics
+  pinocchio::Data data_;            // Pinocchio datas for forward kinematics
+  int foot_ids_[4] = {0, 0, 0, 0};  // Indexes of feet frames
+  Matrix34 pos_feet_;               // Estimated feet positions based on measurements
+  Vector19 q_FK_;                   // Estimated state of the base (height, roll, pitch, joints)
+
   // Methods to compute intersection point
   bool doIntersect_segment(Vector2 const& p1, Vector2 const& q1, Vector2 const& p2, Vector2 const& q2);
   bool onSegment(Vector2 const& p, Vector2 const& q, Vector2 const& r);

scripts/Controller.py

@@ -15,6 +15,8 @@ from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRob
 import libquadruped_reactive_walking as lqrw
 from example_robot_data.robots_loader import Solo12Loader
 
+from solo3D.tools.utils import quaternionToRPY
+
 class Result:
     """Object to store the result of the control loop
     It contains what is sent to the robot (gains, desired positions and velocities,
@@ -143,8 +145,6 @@ class Controller:
         if params.solo3D:
             from solo3D.SurfacePlannerWrapper import SurfacePlanner_Wrapper
             from solo3D.tools.pyb_environment_3D import PybEnvironment3D
-            from solo3D.tools.utils import quaternionToRPY
-            from example_robot_data import load
 
         self.enable_multiprocessing_mip = params.enable_multiprocessing_mip
         self.offset_perfect_estimator = 0.
@@ -168,12 +168,6 @@ class Controller:
             N_sample_ineq = 10  # Number of sample while browsing the curve
             degree = 7  # Degree of the Bezier curve
 
-            # pinocchio model and data, CoM and Inertia estimation for MPC
-            robot = load('solo12')
-            self.data = robot.data.copy()  # for velocity estimation (forward kinematics)
-            self.model = robot.model.copy()  # for velocity estimation (forward kinematics)
-            self.q_neutral = pin.neutral(self.model).reshape((19, 1))  # column vector
-
             self.footTrajectoryGenerator = lqrw.FootTrajectoryGeneratorBezier()
             self.footTrajectoryGenerator.initialize(params, self.gait, self.surfacePlanner.floor_surface,
                                                     x_margin_max_, t_margin_, z_margin_, N_sample, N_sample_ineq,
@@ -453,10 +447,9 @@ class Controller:
                     self.o_targetFootstep[:2, foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
 
         # Update pos, vel and acc references for feet
-        if self.solo3D:  # Bezier curves, needs estimated position of the feet
-            currentPosition = self.computeFootPositionFeedback(self.k, device, self.q_filt_3d, self.v_filt_3d)
+        if self.solo3D:  # Bezier curves
             self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep, self.surfacePlanner.selected_surfaces,
-                                                currentPosition)
+                                                self.q_filt_3d)
         else:
             self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
         # Whole Body Control
@@ -523,7 +516,7 @@ class Controller:
                 self.h_ref += self.vref_filt_mpc[2, 0] * self.dt_wbc
                 self.h_ref = np.clip(self.h_ref, 0.19, 0.26)
                 self.xgoals[3:5, 0] = np.clip(self.xgoals[3:5, 0], [-0.25, -0.17], [0.25, 0.17])"""
-            
+
 
             self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0]  # Velocities (in horizontal frame!)
 
@@ -576,7 +569,7 @@ class Controller:
 
         """if self.k == 1:
             quit()"""
-        
+
         """np.set_printoptions(precision=3, linewidth=300)
         print("---- ", self.k)
         print(self.x_f_mpc[12:24, 0])
@@ -727,65 +720,3 @@ class Controller:
         self.t_mpc = t_mpc - t_planner
         self.t_wbc = t_wbc - t_mpc
         self.t_loop = time.time() - tic
-
-    def computeFootPositionFeedback(self, k, device, q_filt, v_filt):
-        ''' Return the position of the foot using Pybullet feedback, Pybullet feedback with forward dynamics 
-        or Estimator feedback with forward dynamics
-        Args :
-        - k (int) : step indice
-        - q_filt (Arrayx18) : q estimated (only for estimator feedback)
-        - v_vilt (arrayx18) : v estimated (only for estimator feedback)
-        Returns :
-        - currentPosition (Array 3x4)
-        '''
-        currentPosition = np.zeros((3, 4))
-        q_filt_ = np.zeros((19, 1))
-        q_filt_[:3] = q_filt[:3]
-        q_filt_[3:7] = pin.Quaternion(pin.rpy.rpyToMatrix(q_filt[3:6, 0])).coeffs().reshape((4, 1))
-        q_filt_[7:] = q_filt[6:]
-
-        # Current position : Pybullet feedback, directly
-        ##########################
-
-        # linkId = [3, 7 ,11 ,15]
-        # if k != 0 :
-        #     links = pyb.getLinkStates(device.pyb_sim.robotId, linkId , computeForwardKinematics=True , computeLinkVelocity=True )
-
-        #     for j in range(4) :
-        #         self.goals[:,j] = np.array(links[j][4])[:]   # pos frame world for feet
-        #         self.goals[2,j] -= 0.016988                  #  Z offset due to position of frame in object
-        #         self.vgoals[:,j] = np.array(links[j][6])     # vel frame world for feet
-
-        # Current position : Pybullet feedback, with forward dynamics
-        ##########################
-
-        # if k > 0:    # Dummy device for k == 0
-        #     qmes = np.zeros((19, 1))
-        #     revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
-        #     jointStates = pyb.getJointStates(device.pyb_sim.robotId, revoluteJointIndices)
-        #     baseState = pyb.getBasePositionAndOrientation(device.pyb_sim.robotId)
-        #     qmes[:3, 0] = baseState[0]
-        #     qmes[3:7, 0] = baseState[1]
-        #     qmes[7:, 0] = [state[0] for state in jointStates]
-        #     pin.forwardKinematics(self.model, self.data, qmes, v_filt)
-        # else:
-        #     pin.forwardKinematics(self.model, self.data, q_filt_, v_filt)
-
-        # Current position : Estimator feedback, with forward dynamics
-        ##########################
-
-        pin.forwardKinematics(self.model, self.data, q_filt_, v_filt)
-
-        contactFrameId = [10, 18, 26, 34]  # = [ FL , FR , HL , HR]
-
-        for j in range(4):
-            framePlacement = pin.updateFramePlacement(self.model, self.data,
-                                                      contactFrameId[j])  # = solo.data.oMf[18].translation
-            frameVelocity = pin.getFrameVelocity(self.model, self.data, contactFrameId[j], pin.ReferenceFrame.LOCAL)
-
-            currentPosition[:, j] = framePlacement.translation[:]
-            # if k > 0:
-            #     currentPosition[2, j] -= 0.016988                     # Pybullet offset on Z
-            # self.vgoals[:,j] = frameVelocity.linear       # velocity feedback not working
-
-        return currentPosition

src/FootTrajectoryGeneratorBezier.cpp

@@ -79,6 +79,27 @@ void FootTrajectoryGeneratorBezier::initialize(Params& params, Gait& gaitIn, Sur
   x_margin_max_ = x_margin_max_in;
   t_margin_ = t_margin_in;  // 1 % of the curve after critical point
   z_margin_ = z_margin_in;
+
+  // Path to the robot URDF (TODO: Automatic path)
+  const std::string filename =
+      std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+
+  // Build model from urdf (base is not free flyer)
+  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
+
+  // Construct data from model
+  data_ = pinocchio::Data(model_);
+
+  // Update all the quantities of the model
+  VectorN q_tmp = VectorN::Zero(model_.nq);
+  q_tmp(6, 0) = 1.0;  // Quaternion (0, 0, 0, 1)
+  pinocchio::computeAllTerms(model_, data_, q_tmp, VectorN::Zero(model_.nv));
+
+  // Get feet frame IDs
+  foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT"));  // from long uint to int
+  foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+  foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+  foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
 }
 
 void FootTrajectoryGeneratorBezier::updatePolyCoeff_XY(int const& i_foot, Vector3 const& x_init, Vector3 const& v_init,
@@ -488,7 +509,7 @@ void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const&
 }
 
 void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected,
-                                           MatrixN const& currentPosition) {
+                                           VectorN const& q) {
   if ((k % k_mpc) == 0) {
     // Indexes of feet in swing phase
     feet.clear();
@@ -525,13 +546,34 @@ void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep,
     }
   }
 
+  // Update feet position using estimated state, by FK
+  update_position_FK(q);
+
+  // Update desired position, velocities and accelerations for flying feet
   for (int i = 0; i < (int)feet.size(); i++) {
-    position_.col(feet[i]) = currentPosition.col(feet[i]);
+    position_.col(feet[i]) = position_FK_.col(feet[i]);
     updateFootPosition(k, feet[i], targetFootstep.col(feet[i]));
   }
   return;
 }
 
+void FootTrajectoryGeneratorBezier::update_position_FK(VectorN const& q) {
+  // Get position of the feet in world frame, using estimated state q
+  q_FK_.head(3) = q.head(3);
+  q_FK_.block(3, 0, 4, 1) =
+      pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), q(5, 0))).coeffs();
+  q_FK_.tail(12) = q.tail(12);
+
+  // Update model and data of the robot
+  pinocchio::forwardKinematics(model_, data_, q_FK_);
+  pinocchio::updateFramePlacements(model_, data_);
+
+  // Get data required by IK with Pinocchio
+  for (int i = 0; i < 4; i++) {
+    position_FK_.col(i) = data_.oMf[foot_ids_[i]].translation();
+  }
+}
+
 bool FootTrajectoryGeneratorBezier::doIntersect_segment(Vector2 const& p1, Vector2 const& q1, Vector2 const& p2,
                                                         Vector2 const& q2) {
   //  Find the 4 orientations required for