rh5_legs_floating_base.cpp File Reference

#include <solvers/qpoases/QPOasesSolver.hpp>
#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
#include <core/RobotModelConfig.hpp>
#include <scenes/velocity_qp/VelocitySceneQP.hpp>
#include <controllers/CartesianPosPDController.hpp>
#include <tools/JointIntegrator.hpp>
#include <unistd.h>
#include <chrono>

Functions
int	main ()

Function Documentation

main()

int main

(

)

Velocity-based example, Cartesian position control on two 6 dof legs of the RH5 humanoid including floating base and two contact points (feet). In the example the following problem is solved:

\[ \begin{array}{ccc} minimize & \| \mathbf{J}_w\dot{\mathbf{q}} - \mathbf{v}_d \|_2\\ \mathbf{\dot{q}} & & \\ & \mathbf{J}_{c,i}\dot{\mathbf{q}} = 0, \, \forall i& \\ & \dot{\mathbf{q}}_m \leq \dot{\mathbf{q}} \leq \dot{\mathbf{q}}_M& \\ \end{array} \]

where

\[ \mathbf{v}_d = \mathbf{K}_p(\mathbf{x}_r-\mathbf{x}) \]

\(\ddot{\mathbf{q}}\) - Vector of robot joint accelerations
\(\dot{\mathbf{v}}_{d}\) - Desired spatial acceleration / controller output
\(\mathbf{J}_w\) - Weighted task Jacobian
\(\mathbf{W}\) - Diagonal weight matrix
\(\mathbf{J}_{c,i}\) - Contact Jacobian of i-th contact point
\(\mathbf{x}_r, \mathbf{x}\) - Reference pose, actual pose
\(\mathbf{K}_p\) - Derivative gain matrix
\(\mathbf{\dot{q}}_m,\mathbf{\dot{q}}_M\) - Joint velocity limits

The robot CoM is supposed to move to a fixed target pose. The QP is solved using the QPOases solver. Note that the robot has a floating base and there are two rigid contacts with the environment given by the feet contacting the ground floor. The solver computes the joint velocities required to comply with the desired spatial velocities given by the Cartesian controller.