cart_pos_ctrl_qpoases.cpp File Reference

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

Functions
int	main ()

Function Documentation

main()

int main

(

)

Simple Velocity-based example, Cartesian position control on a kuka iiwa 7 dof arm. In contrast to the cart_pos_ctrl_hls example, a QP solver (qpoases) is used to compute the solution. In the example the following problem is solved:

\[ \begin{array}{ccc} minimize & \| \mathbf{J_w\dot{q}}-\mathbf{v}_{d} \|_2& \\ \mathbf{\dot{q}} & & \\ s.t. & \mathbf{\dot{q}}_m \leq \mathbf{\dot{q}} \leq \mathbf{\dot{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 gain matrix
\(\mathbf{\dot{q}}_m,\mathbf{\dot{q}}_M\) - Joint velocity limits

The robot end effector is supposed to move to a fixed target pose. The QP is solved using the QPOases solver. In contrast to the cart_pos_ctrl_hls example, the solver output will always be within the joint velocity limits, which are defined in the kuka iiwa URDF file.