cart_pos_ctrl_hls.cpp File Reference

#include <core/RobotModelConfig.hpp>
#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
#include <scenes/velocity/VelocityScene.hpp>
#include <solvers/hls/HierarchicalLSSolver.hpp>
#include <controllers/CartesianPosPDController.hpp>
#include <unistd.h>

Functions
int	main ()

Function Documentation

main()

int main

(

)

Simple Velocity-based example, Cartesian position control on a kuka iiwa 7 dof arm. The solution is computed using the hierarchical least squares solver (hls), which provides an analytical solution. In the example the following problem is solved:

\[ \begin{array}{ccc} minimize & \| \dot{\mathbf{q}} \|_2& \\ \mathbf{\dot{q}} & & \\ s.t. & \mathbf{J}_w\dot{\mathbf{q}}=\mathbf{v}_{d} \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

The robot end effector is supposed to move to a fixed target pose. As there is only one task, the soluton degrades to a simple pseudo inversion / linear least squares. The task weights are all set to 1 initially, i.e., all task variables are treated with equal priority. The solution resolves the redundancy by minimizing the kinetic energy, i.e., it provides the minimal joint velocities that produce the desired spatial velocity. A 7 dof arm is redundant by 1 dof when considering the task of, e.g., controlling the full pose (position and orientation) of a robot's end effector. In principle, the number of dof can be arbitrary in WBC, as redundancy resolution is automatically handled by the solver.