Corrected maximum joint torque from 30 to 50. Remember to run the script toupdate your model ingazebo!
Changed P to make coman do a step @ 50Hz
Added node to send trajectory to the robot in position recorded in a file
Small changin in the gazebo part. Now maximum allowed velocity is 4 rad/sec. Increasing of PID values
Added dynamic reconfigure. HERE PID changed.