Inverse Kinematics for UR5 Robot Manpulator - C/C++ Codes

If you are looking for C/C++ codes for simulating (Universal Robot) UR5  Robot Manipulator, you can download the codes from this GIT repository.  It contains the codes for the following capabilities:

• C++ interface: Classes / Objects
• forward Kinematics, 
• Inverse Kinematics - Pseudoinverse with null space optimization for joint limit avoidance, Jacobian Transpose method and PD controller for error minimization.
• Analytic as well as Geometric Jacobian 
• Few basic codes for Visual servoing.  
• This code is integrated with GNUPLOT to directly plot the outputs.  For the code to work, you also need to download this gnuplot interface.   
• Maxima source code for symbolic computation of various equations.
  
  

Some of the the screenshots of the plots generated by the codes are shown below:

 

Robot Versus Machine

People keep asking me this question all the time. Let me ask it here myself "What is the difference between a robot and a machine?"

At least we all know that a robot is definitely a machine but what is it that differentiates it from other machines?

I am sure there will be many answers to this questions and many of them would be better than the answer that I am going to give here. But in my opinion, a robot is a machine that exhibits certain aspect of sentient behaviour. Let me explain this.  When I say "sentient behaviour", I include the ability to make mistakes as well. So repeatability is not necessarily an important attribute of a robot. So while calculator is a machine it does not manifest any aspect of human behaviour, apart from the ability to compute with higher accuracy. 

Well it is very difficult to draw a very sharp line between a robot and a machine. But it is the behaviour which defines whether a machine is a robot or not. For instance, a camera in itself is a machine but a camera with a built-in face recognition algorithm may be considered as a robot. While machines are the extension of human faculties, robots exhibit behaviours.

To summarize, a machine may be called a robot if it has the following important attributes:

1. Autonomy: Ability to take decisions without human interference. 
2. Adaptibility: Ability to adapt to changes in the environment. 
3. Learning: It should be able to learn on its own and hence its behaviour should improve over time. 
4. Interact: Interact with other beings and with the environment. 

 These abilities may be available with a varying degree in different machines. In this aspect, we may safely say that a calculator is a simple machine and so is a motor cycle. However, a google car with autonomous navigation ability is a  robot and so is the roomba vacuum cleaner which can the clean the house on its own.

Stageros + roscpp + controlling robot through a C++ program

The objective is to control a simulated robot within the stage environment. This will help us in writing our own control algorithms for controlling the simulated robot. Please follow the steps given below:

#1 Follow the instructions on this page. This package demonstrates how one can run a stage simulation through stageros. Basically tells you to download the package stage_controllers.tar.gz and compile and build the package within a folder where ROS can find it.

$ rosmake stage_controllers

$ rospack find stage_controllers

$ rosrun stage stageros `rospack find stage_controllers`/world/roomba-wander.world

You should be able to see a robot moving around in a stage environment as shown in the picture.

#2 Now we want to stop this robot moving and write a C++ program to provide the necessary motion command.  In order to achieve this, open the file ‘/world/roomba-wander.world’ inside the stage_controllers folder and comment out the line that contains the word “wander” as shown below:

roomba

(              

  # can refer to the robot by this name

  name "roomba"

  pose [ -8 6 0 0 ]

  sicklaser()

#ctrl "wander"

)

If you now execute the above commands you will see a stationary robot within the stage environment. Now we need to write a C++ program to get the robot moving again.

#3 Create a file ‘robctrl.cpp’ inside ‘stage_controllers/src/’ folder that contains the following piece of code:

#include "ros/ros.h" #include "std_msgs/String.h" #include "geometry_msgs/Twist.h" #include int main(int argc, char **argv) {   ros::init(argc, argv, "RobCtrl");   ros::NodeHandle n;   ros::Publisher cmd_pub = n.advertise("cmd_vel",1);   geometry_msgs::Twist cmd_msg;   ros::Rate cycle(10.0);   //Specify the velocity   cmd_msg.linear.x = 0.1;   cmd_msg.linear.y = 0;   cmd_msg.linear.z = 0;   cmd_msg.angular.x = 0;   cmd_msg.angular.y = 0;   cmd_msg.angular.z = 0.2;   while(ros::ok())   {         cmd_pub.publish(cmd_msg);         cycle.sleep();   }   return 0; }

#4 Edit the stage_controllers/manifest.xml to reflect dependency on stage package.

#5 Add the following line to the end of the ‘CMakeLists.txt’ file

rosbuild_add_executable(stage_controllers src/robctrl.cpp)

#6 Build

$ rosmake stage_controllers stage_controllers

# Execute

$ roscore (on one window)

$ rosrun stage stageros `rospack find stage_controllers`/world/roomba-wander.world (on another window)

$ rosrun stage_controllers stage_controllers

You should see the robot rotating in a circle within the stage environment.

This program publishes the velocity values to the topic ‘/cmd_vel’. To know about the available topics where the data could be published, use ‘rostopic’ and ‘rxgraph’ command.

$ rostopic list -v

Published topics:

 * /base_scan [sensor_msgs/LaserScan] 1 publisher

 * /rosout [rosgraph_msgs/Log] 2 publishers

 * /tf [tf/tfMessage] 1 publisher

 * /clock [rosgraph_msgs/Clock] 1 publisher

 * /odom [nav_msgs/Odometry] 1 publisher

 * /rosout_agg [rosgraph_msgs/Log] 1 publisher

 * /cmd_vel [geometry_msgs/Twist] 1 publisher

 * /base_pose_ground_truth [nav_msgs/Odometry] 1 publisher

Subscribed topics:

 * /clock [rosgraph_msgs/Clock] 2 subscribers

 * /rosout [rosgraph_msgs/Log] 1 subscriber

 * /cmd_vel [geometry_msgs/Twist] 1 subscriber

$ rxgraph

ROSCPP + OpenCV Program

Basically, here I am demonstrating how you can start writing your own program with ROS.

Create ROS package. Follow the tutorial link here. I am assuming that ros_workspace folder is added into ROS_PACKAGE_PATH

$ cd ~/ros_workspace

$ roscreate-pkg tbsimctrl std_msgs roscpp opencv2 sensor_msgs cv_bridge image_transport

You may not all the dependencies mentioned above.

$ cd tbsimctrl

$ rospack profile

$ rospack find tbsimctrl

/home/turtlebot/ros_workspace/tbsimctrl

Now create a file “camera_test.cpp” under src folder. The file contains the following piece of code:

#include

#include    //may not be needed

#include  //may not be needed

#include  //may not be needed

#include

#include

#include

#include

using namespace std;

using namespace cv;

int main(int argc, char **argv)

{

  ros::init(argc, argv, "captureimg");

  ros::NodeHandle n;

  CvCapture* capture = cvCaptureFromCAM(-1);

  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 320);

  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 240);

  IplImage *img = 0;

  img = cvQueryFrame(capture);

  int frameH = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);

  int frameW = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);

  int fps = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);

  cout << frameW << “\t” << frameH << “\t” << fps << endl;

  cvNamedWindow("Left", CV_WINDOW_AUTOSIZE);

  for(int n = 1; n= 30; n++)

  {

        img = cvQueryFrame(capture);

        cvShowImage("Left", img);  //show frames while capturing

        int c = cvWaitKey(20);               // wait 20 ms

        if(c == 27)

          break;

  }

  cvDestroyWindow( "Left" );

  cvReleaseCapture(&capture);

  return 0;

}

The above program captures video from a webcam. Save the file and insert the following statement inside the CMakeLists.txt file inside the tbsimctrl package folder. 

rosbuild_add_executable(${PROJECT_NAME} src/camera_test.cpp)

Now compile and build the package using rosmake

$ cmake .

$ rosmake

Make sure that the package is built with 0 failures. Once it is done, you can test the code as follows:

$ rosrun tbsimctrl tbsimctrl

You should be able to see the video from your webcam.

Robotics for HealthCare (R4H)

Robots are needed in healthcare to enhance quality, accessibility and efficiency of healthcare and making them more personalized and affordable for people. Use of Robots in health care is still limited due to several technological challenges involved. Some of the key innovation areas in this field are follows:

- Smart medical Capsules: micro or nano-robots in a capsule that can travel through the body parts carrying out highly precise and localized drug delivery or operations thereby minimizing the risks and increasing the success rates. No working prototype is available so far. People are looking into an application where nanorobots could be used for precision eye surgery. 

- Robotised surgery: Attributes of good surgery like precision, accuracy and repeatability could be enhanced by using robots and automating the surgical process. It is now possible to have a system that can facilitate minimally invasive surgery, remote tele-surgery, preoperative planning, surgical training, intra-operative navigation (image-guided surgery) and surgical simulation all from one place. One such example is "Da Vinci Surgical Robot" from Intuitive Surgical Systems (http://en.wikipedia.org/wiki/Da_Vinci_Surgical_System) , http://www.intuitivesurgical.com/products/davinci_surgical_system/ . 

-Intelligent Prosthetics: Advance control systems and mechatronics could be used for building artificial limbs which could be controlled through brain or the nervous system. It would be possible in future to regain full functionality with artificial limbs. One such example is "C-Leg" from Otto Bock Corporation (http://www.ottobockus.com/), (http://en.wikipedia.org/wiki/Otto_Bock). 

- Robotised Motor Coordination Analysis and Therapy: Robots could be used for providing physical therapy in the treatment of patients with traumatised motor control, for example following stroke. These robots could be programmed to provide different kinds of physical therapies which can improve the restoration of central motor coordination of the nervous system. One such example is the Lokomat System from Hocoma (http://www.hocoma.com/products/lokomat/).

- Robot assisted mental, cognitive and social therapy: Robots could be used for increasing the social interaction of people with mental, social or cognitive handicap (autistic children and elders with dementia) by enhancing their social skills through therapeutic exercises. The robots could be programmed to generative communicative reactions, participate in playing games or stimulate friendly face expressions. One such system is Kaspar (http://kaspar.herts.ac.uk/). There is a research group at University of Delaware which looks into developing robots that can be used for rehabilitating infants with disabled mobility. http://www.udel.edu/research/media/babiesrobots.html 

- Robotised patient monitoring system: There is a great need for robots which not only helps the elder people to carry out daily routines but also keeps monitoring the vitals of the patients. It can assist the person in walking, providing support in sitting, sleeping or getting up from bed, May assist in fetching things and can also interact through speech or vision. It can identify urgency situations and can inform nearby hospital. The doctors can also communicate with this robot remotely to collect or analyse the stored patient data etc. One example of such systems is Care-o-robot from Fraunhofer IPA (http://www.ros.org/wiki/Robots/Care-O-bot) (http://www.care-o-bot.de/english/). 


Reference: 
http://ec.europa.eu/information_society/activities/health/docs/studies/robotics_healthcare/robotics-in-healthcare.pdf
 

Player/Stage on Ubuntu 12.04 LTS 64-bit

Player Installation: 

The instructions given on this link worked for me with some changes.Install the necessary dependencies:

$ sudo apt-get install linux-headers-generic autotools-dev build-essential cmake cpp libboost-signals1.46.1 libboost-signals1.46-dev libboost-thread1.46.1 libboost-thread1.46-dev libcv-dev libgdk-pixbuf2.0-0 libgdk-pixbuf2.0-dev libopencv-features2d-dev libopencv-calib3d-dev libgnomecanvas2-0 libgnomecanvas2-dev libgsl0-dev libgtk2.0-dev libjpeg-dev libtool libxmu-dev swig python2.7-dev libcv-dev libcvaux-dev libhighgui-dev

Download the source code:

$ wget http://sourceforge.net/projects/playerstage/files/Player/3.0.2/player-3.0.2.tar.gz

Build and install as usual. See this link for more information.  Now set the following two variables:

$ export PKG_CONFIG_PATH=/usr/local/lib64/pkgconfig

You may like to put this into your ~/.bashrc file to avoid repeating every time you log into your system.

Stage Installation: 

Install the dependencies:

$ sudo apt-get install freeglut3 freeglut3-dev libfltk1.1 libfltk1.1-dev libltdl7 libltdl-dev libpng12-dev libpng12-0

Usual building process should work, this link (same as given in the beginning) tells you to make some changes in the file 'CMakeLists.txt'. 

After the installation, you should be able to execute the following command without any error:

$ cd ~/Downloads/Stage-3.2.2-Source/worlds

$ player simple.cfg

You may get error if it fails to find the libstage plugin. Just 'locate' libstage library and add this folder into /etc/ld.so.conf and run 'ldconfig'.

While installing stage, make sure that the cmake command finds the Player Installation. You should be able to run the following command:

$ pkg-config playercore --libs 

and you should see something like this:

-L/usr/local/lib64 -lplayercore -lpthread -lltdl -ldl -lplayerinterface -lplayercommon

Player/Stage Not working on Debian Squeeze

Hi all!

I am trying to install Player/Stage on Debian 6.0. I followed the steps given here. I don't get any error during installation. However, I get error when I test it as shown below. If you have any idea to fix this, please do let me know.

-------------------------
$ player simple.cfg
Registering driver
Player v.3.0.2

* Part of the Player/Stage/Gazebo Project [http://playerstage.sourceforge.net].
* Copyright (C) 2000 - 2009 Brian Gerkey, Richard Vaughan, Andrew Howard,
* Nate Koenig, and contributors. Released under the GNU General Public License.
* Player comes with ABSOLUTELY NO WARRANTY.  This is free software, and you
* are welcome to redistribute it under certain conditions; see COPYING
* for details.

invoking player_driver_init()...
 Stage driver plugin init

 ** Stage plugin v3.2.2 **
 * Part of the Player Project [http://playerstage.sourceforge.net]
 * Copyright 2000-2009 Richard Vaughan, Brian Gerkey and contributors.
 * Released under the GNU General Public License v2.

success
 Stage plugin:  6665.simulation.0 is a Stage world*** glibc detected *** player: corrupted double-linked list: 0x0000000000ca2b10 ***
======= Backtrace: =========
/lib/libc.so.6(+0x71ad6)[0x7fee39168ad6]
/lib/libc.so.6(+0x71f4d)[0x7fee39168f4d]
/lib/libc.so.6(+0x74254)[0x7fee3916b254]
/lib/libc.so.6(__libc_malloc+0x70)[0x7fee3916d930]
/usr/lib/libstdc++.so.6(_Znwm+0x1d)[0x7fee399bb6bd]
/usr/local/lib/stageplugin.so(_ZN19InterfaceSimulationC1E14player_devaddrP9StgDriverP10ConfigFilei+0xd5)[0x7fee37cdd735]
/usr/local/lib/stageplugin.so(_ZN9StgDriverC1EP10ConfigFilei+0x279)[0x7fee37cdbaa9]
/usr/local/lib/stageplugin.so(_Z14StgDriver_InitP10ConfigFilei+0x34)[0x7fee37cdbd94]
/usr/local/lib64/libplayercore.so.3.0(_ZN10ConfigFile11ParseDriverEi+0x22c)[0x7fee3b0e9f70]
/usr/local/lib64/libplayercore.so.3.0(_ZN10ConfigFile15ParseAllDriversEv+0x4f)[0x7fee3b0e9d15]
player(main+0x498)[0x40267c]
/lib/libc.so.6(__libc_start_main+0xfd)[0x7fee39115c4d]
player[0x402129]
======= Memory map: ========
00400000-00404000 r-xp 00000000 08:06 222792                             /usr/local/bin/player
00604000-00605000 rw-p 00004000 08:06 222792                             /usr/local/bin/player
00c88000-00ca9000 rw-p 00000000 00:00 0                                  [heap]
7fee2c000000-7fee2c021000 rw-p 00000000 00:00 0
7fee2c021000-7fee30000000 ---p 00000000 00:00 0
7fee33b55000-7fee33b7b000 r-xp 00000000 08:06 212277                     /usr/lib/libexpat.so.1.5.2
7fee33b7b000-7fee33d7b000 ---p 00026000 08:06 212277                     /usr/lib/libexpat.so.1.5.2
7fee33d7b000-7fee33d7d000 rw-p 00026000 08:06 212277                     /usr/lib/libexpat.so.1.5.2
7fee33d7d000-7fee33d86000 r-xp 00000000 08:06 213529                     /usr/lib/libXrender.so.1.3.0
7fee33d86000-7fee33f86000 ---p 00009000 08:06 213529                     /usr/lib/libXrender.so.1.3.0
7fee33f86000-7fee33f87000 rw-p 00009000 08:06 213529                     /usr/lib/libXrender.so.1.3.0
7fee33f87000-7fee3400a000 r-xp 00000000 08:06 213498                     /usr/lib/libfreetype.so.6.6.0
7fee3400a000-7fee34209000 ---p 00083000 08:06 213498                     /usr/lib/libfreetype.so.6.6.0
7fee34209000-7fee3420f000 rw-p 00082000 08:06 213498                     /usr/lib/libfreetype.so.6.6.0
7fee3420f000-7fee34214000 r-xp 00000000 08:06 213517                     /usr/lib/libXdmcp.so.6.0.0
7fee34214000-7fee34413000 ---p 00005000 08:06 213517                     /usr/lib/libXdmcp.so.6.0.0
7fee34413000-7fee34414000 rw-p 00004000 08:06 213517                     /usr/lib/libXdmcp.so.6.0.0
7fee34414000-7fee34416000 r-xp 00000000 08:06 213515                     /usr/lib/libXau.so.6.0.0
7fee34416000-7fee34616000 ---p 00002000 08:06 213515                     /usr/lib/libXau.so.6.0.0
7fee34616000-7fee34617000 rw-p 00002000 08:06 213515                     /usr/lib/libXau.so.6.0.0
7fee34617000-7fee34619000 r-xp 00000000 08:06 213919                     /usr/lib/libXinerama.so.1.0.0
7fee34619000-7fee34818000 ---p 00002000 08:06 213919                     /usr/lib/libXinerama.so.1.0.0
7fee34818000-7fee34819000 rw-p 00001000 08:06 213919                     /usr/lib/libXinerama.so.1.0.0
7fee34819000-7fee3484c000 r-xp 00000000 08:06 213511                     /usr/lib/libfontconfig.so.1.4.4
7fee3484c000-7fee34a4c000 ---p 00033000 08:06 213511                     /usr/lib/libfontconfig.so.1.4.4
7fee34a4c000-7fee34a4e000 rw-p 00033000 08:06 213511                     /usr/lib/libfontconfig.so.1.4.4
7fee34a4e000-7fee34a62000 r-xp 00000000 08:06 213891                     /usr/lib/libXft.so.2.1.13
7fee34a62000-7fee34c61000 ---p 00014000 08:06 213891                     /usr/lib/libXft.so.2.1.13
7fee34c61000-7fee34c62000 rw-p 00013000 08:06 213891                     /usr/lib/libXft.so.2.1.13
7fee34c62000-7fee34c85000 r-xp 00000000 08:06 213831                     /usr/lib/libjpeg.so.62.0.0
7fee34c85000-7fee34e84000 ---p 00023000 08:06 213831                     /usr/lib/libjpeg.so.62.0.0
7fee34e84000-7fee34e85000 rw-p 00022000 08:06 213831                     /usr/lib/libjpeg.so.62.0.0
7fee34e85000-7fee34eaa000 r-xp 00000000 08:06 25872                      /lib/libpng12.so.0.44.0
7fee34eaa000-7fee350aa000 ---p 00025000 08:06 25872                      /lib/libpng12.so.0.44.0
7fee350aa000-7fee350ab000 rw-p 00025000 08:06 25872                      /lib/libpng12.so.0.44.0
7fee350ab000-7fee350c7000 r-xp 00000000 08:06 213519                     /usr/lib/libxcb.so.1.1.0
7fee350c7000-7fee352c6000 ---p 0001c000 08:06 213519                     /usr/lib/libxcb.so.1.1.0
7fee352c6000-7fee352c7000 rw-p 0001b000 08:06 213519                     /usr/lib/libxcb.so.1.1.0
7fee352c7000-7fee352cb000 r-xp 00000000 08:06 24160                      /lib/libuuid.so.1.3.0
7fee352cb000-7fee354ca000 ---p 00004000 08:06 24160                      /lib/libuuid.so.1.3.0
7fee354ca000-7fee354cb000 rw-p 00003000 08:06 24160                      /lib/libuuid.so.1.3.0
7fee354cb000-7fee354d5000 r-xp 00000000 08:06 214122                     /usr/lib/libdrm.so.2.4.0
7fee354d5000-7fee356d5000 ---p 0000a000 08:06 214122                     /usr/lib/libdrm.so.2.4.0
7fee356d5000-7fee356d6000 rw-p 0000a000 08:06 214122                     /usr/lib/libdrm.so.2.4.0
7fee356d6000-7fee356db000 r-xp 00000000 08:06 213909                     /usr/lib/libXfixes.so.3.1.0
7fee356db000-7fee358da000 ---p 00005000 08:06 213909                     /usr/lib/libXfixes.so.3.1.0
7fee358da000-7fee358db000 rw-p 00004000 08:06 213909                     /usr/lib/libXfixes.so.3.1.0
7fee358db000-7fee358dd000 r-xp 00000000 08:06 213915                     /usr/lib/libXdamage.so.1.1.0
7fee358dd000-7fee35adc000 ---p 00002000 08:06 213915                     /usr/lib/libXdamage.so.1.1.0
7fee35adc000-7fee35add000 rw-p 00001000 08:06 213915                     /usr/lib/libXdamage.so.1.1.0
7fee35add000-7fee35ae2000 r-xp 00000000 08:06 214124                     /usr/lib/libXxf86vm.so.1.0.0
7fee35ae2000-7fee35ce1000 ---p 00005000 08:06 214124                     /usr/lib/libXxf86vm.so.1.0.0Aborted