• This must be done in each terminal in which you want to use ROS

$ source /opt/ros/b3b33rob/setup.bash

You can also add this line to ~/.bashrc to avoid this step.

• Following command will start the robot driver and the gripper driver, Rviz and optionally the camera driver

# for the RV-6SDL with camera
$ roslaunch b3b33rob_ros start_rv6sdl.launch camera:=true
 
# for the RV-6SDL without camera
$ roslaunch b3b33rob_ros start_rv6sdl.launch camera:=false
$ roslaunch b3b33rob_ros start_rv6sdl.launch

• There is also option to start fake driver, which will provide same interface as the real driver but without actual connection to the robot, so you can test your program and see what is happening in rviz but the real robot won't move.

# for the RV-6SDL with camera
$ roslaunch b3b33rob_ros start_fake_rv6sdl.launch camera:=true
 
# for the RV-6SDL without camera
$ roslaunch b3b33rob_ros start_fake_rv6sdl.launch camera:=false
$ roslaunch b3b33rob_ros start_fake_rv6sdl.launch

• Then in other terminal (do not forget to source the environment) you can start your program:

$ python /opt/ros/b3b33rob/share/b3b33rob_ros/scripts/test_getposition.py 

Robot python interface object which covers ROS interface. The object integrates methods for reading position, moving robot, and IKT/DKT functions.

• you can find these examples in /opt/ros/b3b33rob/share/b3b33rob_ros/scripts
• test_getposition.py - just read the current position no motion.

# main rospy package
import rospy
 
# import pi as ros uses radians
from numpy import pi
 
# import the robot interface
from mitsubishi_arm_student_interface.mitsubishi_robots import Mitsubishi_robot
 
 
if __name__=='__main__':
 
    # Initialize robot interface class
    robot = Mitsubishi_robot()
 
    # Get and print current position of the robot in joint coordinates
    j = robot.get_joint_values()
    print 'Joint position:'
    print j
 
    # Get and print current position of the end effector in the cartesian coordinates
    c = robot.get_position()
    print 'Cartesian position:'
    print c

• test_gotoposition.py - be aware the robot will move.

import rospy
from numpy import pi
from mitsubishi_arm_student_interface.mitsubishi_robots import Mitsubishi_robot
 
 
if __name__=='__main__':
 
    robot = Mitsubishi_robot()
 
    # Set maximal relative speed (it is recomended to decrease the speed for testing)
    robot.set_max_speed(0.3);
 
    # Move to to position given in joint coordinates
    print 'Moving to base position'
    robot.set_joint_values([0,0,pi/2,0,pi/2,0])
 
    # Move a bit more
    print 'Move'
    robot.set_joint_values([pi/4,0,pi/2,0,pi/2,0])
    robot.set_joint_values([-pi/4,0,pi/2,0,pi/2,0])
    robot.set_joint_values([0,0,pi/2,0,pi/2,0])

• test_trajectory.py - test trajectory through several waypoints.

import numpy as np
from numpy import pi
from mitsubishi_arm_student_interface.mitsubishi_robots import Mitsubishi_robot
 
if __name__=='__main__':
    robot = Mitsubishi_robot()
    robot.set_max_speed(0.3)
 
    # Prepare waypoints in the joint coordinates
    waypoints = []
    waypoints.append([pi/4,0,pi/4,0,0,0])
    waypoints.append([pi/8,0,3*pi/8,0,0,0])
    waypoints.append([0,0,pi/2,0,pi/2,0])
 
    # Execute trajectory that will go through all waypoints
    robot.execute_joint_trajectory(waypoints)

• test_gripper.py - Open and close the gripper.

import numpy as np
from numpy import pi
from mitsubishi_arm_student_interface.mitsubishi_robots import Mitsubishi_robot
 
if __name__=='__main__':
    robot = Mitsubishi_robot()
 
    # Open gripper
    robot.set_gripper('open')
 
    # Close gripper
    robot.set_gripper('close')

get_joint_values(self) - returns complete list of current joint values in form J=[J1,J2,J3,J4,J5,J6]. In case gripper is attached, the form is P = [J1,J2,J3,J4,J5,J6,gripper].

get_position(self) - returns the current position of the endpoint of the robot in Cartesian coordinates as a slope vector in form P = [X,Y,Z,Rz1,Rx,Rz2].

set_max_speed(self, speed) - sets the maximal fraction (0 … 1.0) of speed to execute the motion. The maximal speed cannot be higher than the maximal allowed speed. The method returns True if succeeded, else returns False.

set_joint_values(self, joints) - sets joint values to the robot as joints input must be a list. If the list of length n⇐7 (7 in case of gripper attached, 6 otherwise) is provided only n joints are set. If the gripper is attached and joint length is 7, the gripper is set too, else only arm joints are set. The method returns True if succeeded, else returns False.

set_gripper(self, position) - Sets gripper open, closed, or something between. Position input can be string 'open', string 'close', or float number. The method returns True if succeded, else returns False.

execute_joint_trajectory(self, waypoints) - executes trajectory given by waypoints in joint coordinates waypoints is a list of joint values if form waipoints = [ [J1,J2,J3,J4,J5,J6],… ]. After execution wait for 0.4s to make sure the robot arrived at the destination pose. The method returns True if succeeded, else returns False.

execute_cart_trajectory(self, waypoints) - executes trajectory given by waypoints in cartesian coordinates waypoints is a list of NumPy array of coordinates if form P = numpy.array([ [X],[Y],[Z],[Rz1],[Rx],[Rz2] ]) (Euler angles rotation). After execution wait for 0.4s to make sure the robot arrived at the destination pose. The method returns True if succeeded, else returns False.

dkt(self, J) - Returns cartesian position of end-point of the robot for given joint values in form J = [J1,J2,J3,J4,J5,J6] output is in form in form X = numpy.array([ [X],[Y],[Z],[Rz1],[Rx],[Rz2] ])

ikt(self, X) - The method returns list all joint coordinates to reach given position in form X = numpy.array([ [X],[Y],[Z],[Rz1],[Rx],[Rz2] ]) (Euler angles rotation]. If soluion exists the output has form J = [ [J1,J2,J3,J4,J5,J6],… ]. If infinity solution exists, the joint value, for which infinity solutions exists is marked with char 'i' (f.e.: J = [ [J1,J2,J3,'i',J5,'i'],… ]). If no solution exists returns empty list. This function ignores collisions!

inf_ikt(self, X, J) - Returns one solution in case of infinity solutions for position X in fom X = numpy.array([ [X],[Y],[Z],[Rz1],[Rx],[Rz2] ]) (Euler angles rotation) input J is a list of joint values in form J=[J1,J2,J3,J4,J5,J6], where one of the joint variables is marked with 'i' for which solution is to be found in list J can be only one 'i'. If no solution is found or the input is incorrect returns an empty list, else returns a list of joint values in form J=[J1,J2,J3,J4,J5,J6].