From c0d85d5c688c30221871af24c3b31ac7c33eaaa0 Mon Sep 17 00:00:00 2001
From: thomascbrs <thomas.corberes@laposte.net>
Date: Mon, 25 Oct 2021 13:59:46 +0100
Subject: [PATCH] Estimation of the feet by FK in Bezier planner, in cpp,
removed from controller
---
include/qrw/FootTrajectoryGeneratorBezier.hpp | 36 +++++++--
scripts/Controller.py | 81 ++-----------------
src/FootTrajectoryGeneratorBezier.cpp | 46 ++++++++++-
3 files changed, 81 insertions(+), 82 deletions(-)
diff --git a/include/qrw/FootTrajectoryGeneratorBezier.hpp b/include/qrw/FootTrajectoryGeneratorBezier.hpp
index ad5b1a9b..aa9a8781 100644
--- a/include/qrw/FootTrajectoryGeneratorBezier.hpp
+++ b/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);
diff --git a/scripts/Controller.py b/scripts/Controller.py
index fa207be2..64a2f07c 100644
--- a/scripts/Controller.py
+++ b/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
diff --git a/src/FootTrajectoryGeneratorBezier.cpp b/src/FootTrajectoryGeneratorBezier.cpp
index 05c155a4..e9032d24 100644
--- a/src/FootTrajectoryGeneratorBezier.cpp
+++ b/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
--
GitLab