cart_pos_ctrl.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 <tasks/SpatialVelocityTask.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 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. The solver output will always be within the joint velocity limits, which are defined in the kuka iiwa URDF file.