Create your first OROCOS controller¶
Using lwr_create_pkg
¶
Using the lwr_project_creator utility, we can generate a (very) simple controller for our robot.
Generate the controller¶
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
lwr_create_pkg my_controller -c MyController
cd ~/catkin_ws
catkin config --init --extend ~/isir/lwr_ws/devel
Build the controller¶
cd ~/catkin_ws
catkin build
# add the controller to the ros package path
source ~/catkin_ws/devel/setup.bash
Launch it :
roslaunch my_controller run.launch sim:=true
General note on controllers¶
This generated controller is “kuka-lwr-free” and as a general rule you should avoid to include kuka-lwr stuff in your code.
You should just see the robot as an *Effort interface*,i.e, you should only listen to the current state (q,qdot) and publish torques.
Recommended ports are :
Robot input / Controller output: - JointTorqueCommand
- (JointPositionCommand)
Robot Output / Controller intput: - JointPosition
- JointVelocity
- (JointTorque)
The complete list of LWR ports :
curl --silent https://raw.githubusercontent.com/kuka-isir/lwr_hardware/master/lwr_fri/src/FRIComponent.cpp | grep -oP 'addPort\( *\"\K\w+'
CartesianImpedanceCommand
CartesianWrenchCommand
CartesianPositionCommand
JointImpedanceCommand
JointPositionCommand
JointTorqueCommand --> To Controller
toKRL --> KRL Tool
KRL_CMD --> KRL Tool
fromKRL --> KRL Tool
CartesianWrench
RobotState --> KRL Tool
FRIState --> KRL Tool
JointVelocity --> To Controller
CartesianVelocity
CartesianPosition
MassMatrix
Jacobian
JointTorque --> To Controller
JointTorqueAct
GravityTorque
JointPosition --> To Controller
JointPositionFRIOffset
The controller uses rtt_ros_kdl_tools::ChainUtils to create an “arm” object. This arm loads the robot_description from the ROS parameter server (you can use the provided launch file that helps you start everything), then create a few KDL chain, solvers etc to compute forward kinematics, jacobians etc.
Functions available can be found here.
Inverse Kinematics is included in ChainUtils via Trac IK .