rh5v2.cpp File Reference

#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
#include <solvers/qpoases/QPOasesSolver.hpp>
#include <scenes/acceleration_tsid/AccelerationSceneTSID.hpp>
#include <tools/JointIntegrator.hpp>
#include <controllers/CartesianPosPDController.hpp>
#include <unistd.h>

Functions
int	main ()

Function Documentation

main()

int main

(

)

Acceleration-based example, Cartesian position control of the RH5v2 arms (fixed base, no contacts). In the example the following problem is solved:

\[ \begin{array}{ccc} minimize & \| \mathbf{J}\ddot{\mathbf{q}} - \dot{\mathbf{v}}_d + \dot{\mathbf{J}}\dot{\mathbf{q}}\|_2\\ \mathbf{\ddot{q}},\mathbf{\tau} & & \\ s.t. & \mathbf{H}\mathbf{\ddot{q}} - \mathbf{\tau} - \mathbf{J}_{c,i}^T\mathbf{f} = -\mathbf{h}, \, \forall i & \\ & \mathbf{\tau}_m \leq \mathbf{\tau} \leq \mathbf{\tau}_M& \\ \end{array} \]

where

\[ \dot{\mathbf{v}}_d = \dot{\mathbf{v}}_r + \mathbf{K}_d(\mathbf{v}_r-\mathbf{v}) + \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{\tau}\) - actuation forces/torques
\(\mathbf{J}\) - task Jacobian
\(\mathbf{H}\) - Joint space inertia matrix
\(\mathbf{h}\) - bias forces/torques
\(\mathbf{\tau}_m,\mathbf{\tau}_M\) - Joint force/torque limits
\(\mathbf{x}_r, \mathbf{x}\) - Reference pose, actual pose
\(\mathbf{v}_r, \mathbf{v}\) - Reference spatial velocity, actual spatial velocity
\(\dot{\mathbf{v}}_r\) - Reference spatial acceleration (feed forward)
\(\mathbf{K}_p, \mathbf{K}_d\) - Proportional, derivative gain matrix

Two tasks are implemented: The left end effector is supposed to follow a sinusoidal trajectory, while the right end effector is supposed to maintain a fixed pose. Note that the tasks share the torso joints (BodyPitch/Roll/Yaw). The QP is solved using the QPOases solver. The solver computes the joint accelerations and forces/torques required to generate the desired spatial accelerations given by the Cartesian controllers.