rh5_single_leg.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 <unistd.h>

Functions
int	main ()

Function Documentation

main()

int main

(

)

Velocity-based example, Cartesian position control on 6 dof leg of the RH5 humanoid (fixed base/fully actuated, serial robot model). 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}} & & \\ s.t. & \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}) \]

\(\dot{\mathbf{q}}\) - Vector of robot joint velocities
\(\mathbf{v}_{d}\) - Desired spatial velocity / controller output
\(\mathbf{J}_w=\mathbf{W}\mathbf{J}\) - Weighted task Jacobian
\(\mathbf{W}\) - Diagonal weight matrix
\(\mathbf{x}_r, \mathbf{x}\) - Reference pose, actual pose
\(\mathbf{K}_p\) - Proportional feed forward gain matrix
\(\mathbf{\dot{q}}_m,\mathbf{\dot{q}}_M\) - Joint velocity limits

The robot ankle is supposed to move to a fixed target pose. The QP is solved using the QPOases solver. Note that the robot has a fixed base here and there are no rigid contacts that have to be considered. In the example, the Hyrodyn robot model is used, however, with a serial URDF model, i.e., the output velocities are in independent joint space.