OpenManipulator with Moveit!

Skill LevelArea of FocusOperating SystemPlatform/Hardware
IntermediateComputer Vision, Embeded, Robotics, Smart CitiesUbuntu / Ubuntu CoreQualcomm Robotics RBx Dev Kit

The OpenMANIPULATOR has hardware compatibility with the Qualcomm® Robotics RB5 Development Kit. Users can also control it more easily by linking it with the MoveIt! package. This project is designed to control the OpenMANIPULATOR arm on the Qualcomm Robotics RB5 platform with Moveit!

The main objective for the project is to control the OpenMANIPULATOR arm using the Qualcomm Robotics RB5 development kit. The project is designed to get you started with the robotics arm, and with the Qualcomm® QRB5165 processor onboard the development kit, you can start building artificial intelligence (AI) supported robotics apps.

Materials Required / Parts List / Tools

Source Code / Source Examples / Application Executable

Additional Resources

Build / Assembly Instructions

1. Hardware Setup

2. Setup OpenCR

On an ubuntu PC, connect the OpenCR board to the PC with USB, and run below command on the PC

wget https://github.com/ROBOTIS-GIT/OpenCR-Binaries/raw/master/turtlebot3/ROS2/latest/opencr_update.tar.bz2
tar -xjf ./opencr_update.tar.bz2
cd ~/opencr_update && ./update.sh /dev/ttyACM0 waffle.opencr

For details please refer: http://emanual.robotis.com/docs/en/platform/openmanipulator_x/ros_setup/#install-ros-packages

3. Install ROS1.0 and Moveit on Qualcomm Robotics RB5 Development kit (target device)

3.1. Enable Wi-Fi

Change your local Wi-Fi's name and password in /data/misc/wifi/wpa_supplicant.conf file as shown below and reset the Robotics RB5 Development Kit

# Only WPA-PSK is used. Any valid cipher combination is accepted.
ctrl_interface=/var/run/wpa_supplicant
network={
#WPA-PSK
ssid="your-ssid-name"
key_mgmt=WPA-PSK
pairwise=TKIP CCMP
group=TKIP CCMP
psk="12345678"
}

3.2. Install dependent packages

root@qrb5165-rb5:/# apt-get update
root@qrb5165-rb5:/# apt-get install openssh-server
root@qrb5165-rb5:/# echo "PermitRootLogin yes" >> /etc/ssh/sshd_config
root@qrb5165-rb5:/# /etc/init.d/ssh restart

3.3. Install ROS1.0

root@qrb5165-rb5:/# apt-get install lsb-core -y
root@qrb5165-rb5:/# sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
root@qrb5165-rb5:/# apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B17xxx…
root@qrb5165-rb5:/# apt update
root@qrb5165-rb5:/# apt install ros-melodic-desktop-full
root@qrb5165-rb5:/# apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential git
root@qrb5165-rb5:/# rosdep init
root@qrb5165-rb5:/# rosdep update
root@qrb5165-rb5:/# echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
root@qrb5165-rb5:/# source ~/.bashrc

3.4. Install Moveit!

Use the following commands on the target device to download and install libraries for Moveit OpenMANIPULATOR robotic arm.

root@qrb5165-rb5:/# mdkir ~/catkin_ws/src/
root@qrb5165-rb5:/# cd ~/catkin_ws/src/
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/open_manipulator.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/open_manipulator_msgs.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/open_manipulator_simulations.git -b melodic-devel
root@qrb5165-rb5:/# git clone https://github.com/ROBOTIS-GIT/robotis_manipulator.git -b melodic-devel
root@qrb5165-rb5:/# apt-get install ros-melodic-smach* ros-melodic-ar-track-alvar ros-melodic-ar-track-alvar-msgs
root@qrb5165-rb5:/# cd ~/catkin_ws && catkin_make

• Save the following as “open_manipulator_controller.launch" and replace ./src/open_manipulator/open_manipulator_controller/launch/open_manipulator_controller.launch on your target device

<launch>
<arg name="use_robot_name" default="open_manipulator"/>
<arg name="dynamixel_usb_port" default="/dev/ttyACM0"/>
<arg name="dynamixel_baud_rate" default="1000000"/>
<arg name="control_period" default="0.010"/>
<arg name="use_platform" default="true"/>
<arg name="use_moveit" default="false"/>
<arg name="planning_group_name" default="arm"/>
<arg name="moveit_sample_duration" default="0.050"/>
<group if="$(arg use_moveit)">
<include file="$(find open_manipulator_controller)/launch/open_manipulator_moveit.launch">
<arg name="robot_name" value="$(arg use_robot_name)"/>
<arg name="sample_duration" value="$(arg moveit_sample_duration)"/>
</include>
</group>
<node name="$(arg use_robot_name)" pkg="open_manipulator_controller" type="open_manipulator_controller" output="screen" args="$(arg dynamixel_usb_port) $(arg dynamixel_baud_rate)">
<param name="using_platform" value="$(arg use_platform)"/>
<param name="using_moveit" value="$(arg use_moveit)"/>
<param name="planning_group_name" value="$(arg planning_group_name)"/>
<param name="control_period" value="$(arg control_period)"/>
<param name="moveit_sample_duration" value="$(arg moveit_sample_duration)"/>
</node>
<node name="rb5_moveit" pkg="open_manipulator_controller" type="rb5_moveit_open_manipulator.py" output="screen">
</node>
</launch>

• Save the following text as “open_manipulator_moveit.launch" and replace ./src/open_manipulator/open_manipulator_controller/launch/open_manipulator_moveit.launch on your target device

<launch>
<arg name="robot_name" default="open_manipulator"/>
<arg name="sample_duration" default="0.010"/>
<arg name="debug" default="false" />
<!—Load the URDF, SRDF and other .yaml configuration files on the param server 
<include file="$(find open_manipulator_moveit)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
&lt!—We do not have a robot connected, so publish fake joint states 
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false"/>
<rosparam param="source_list" subst_value="true">[“$(arg robot_name)/joint_states"]</rosparam>
</node>
<!—Given the published joint states, publish tf for the robot links 
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!—Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) 
<include file="$(find open_manipulator_moveit)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="false"/>
<arg name="fake_execution" value="false"/>
<arg name="info" value="false"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="sample_duration" value="$(arg sample_duration)"/>
</include>
</launch>

• Save the following text as “rb5_moveit_open_manipulator.py" and replace ./devel/lib/open_manipulator_controller/rb5_moveit_open_manipulator.py on your target device

#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import time
import open_manipulator_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from open_manipulator_msgs.srv import *
home_joints = [0, -1.052, 0.354, 0.703]
pre_target1_joints = [0.318, -0.548, 0.769, -0.242]
target1_joints = [0.321, 0.020, 0.314, -0.301]
pre_target2_joints = [1.75027215481, -1.21644675732, 0.639670014381, 0.65961176157]
target2_joints = [1.74567019939, -0.449456393719, 0.845223426819, -0.294524312019]
open_joint = 0.01
close_joint = -0.01
class MoveGroupPythonIntefaceTutorial(object):
“""MoveGroupPythonIntefaceTutorial"""
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(‘move_group_python_interface_tutorial’, anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot’s
## kinematic model and the robot’s current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot’s internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group’s name to “panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = “arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(‘/move_group/display_planned_path’,
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print “============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print “============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print “============ Available Planning Groups:", robot.get_group_names(
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print “============ Printing robot state"
print robot.get_current_state()
print “"
# Misc variables
self.box_name = ‘’
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def go_to_joint_state(self, joint_goal):
move_group = self.move_group
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
time.sleep(3.5)
return 0
def control_grapper(self, joint_value):
rospy.wait_for_service(‘/open_manipulator/goal_tool_control’)
set_joint_position = rospy.ServiceProxy(‘/open_manipulator/goal_tool_control’, SetJointPosition)
msg = open_manipulator_msgs.msg.JointPosition()
msg.joint_name.append(“gripper")
msg.position.append(joint_value)
resp1 = set_joint_position(“arm", msg, 5)
time.sleep(1.5)
def pick1(self):
global home_joints
global pre_target1_joints
global target1_joints
global open_joint
global close_joint
self.go_to_joint_state(home_joints)
self.go_to_joint_state(pre_target1_joints)
self.control_grapper(open_joint)
self.go_to_joint_state(target1_joints)
self.control_grapper(close_joint)
def place1(self):
global home_joints
global pre_target2_joints
global target2_joints
self.go_to_joint_state(home_joints)
self.go_to_joint_state(target2_joints)
self.control_grapper(open_joint)
self.go_to_joint_state(pre_target2_joints)
self.control_grapper(close_joint)
def pick2(self):
global home_joints
global pre_target1_joints
global target1_joints
global open_joint
global close_joint
self.go_to_joint_state(home_joints)
self.go_to_joint_state(pre_target2_joints)
self.control_grapper(open_joint)
self.go_to_joint_state(target2_joints)
self.control_grapper(close_joint)
def place2(self):
global home_joints
global pre_target2_joints
global target2_joints
self.go_to_joint_state(home_joints)
self.go_to_joint_state(target1_joints)
self.control_grapper(open_joint)
self.go_to_joint_state(pre_target1_joints)
self.control_grapper(close_joint)
def main():
try:
tutorial = MoveGroupPythonIntefaceTutorial()
while(True):
tutorial.pick1()
tutorial.place1()
tutorial.pick2()
tutorial.place2()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == ‘__main__’:
main()

Run the following commands on target device.

root@qrb5165-rb5:/# wget https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/master/turtlebot3_bringup/99-turtlebot3-cdc.rules -O /etc/udev/rules.d/99-turtlebot3-cdc.rules
root@qrb5165-rb5:/# udevadm control --reload-rules
root@qrb5165-rb5:/# udevadm trigger
root@qrb5165-rb5:/# echo "source catkin_ws/devel/setup.bash" >> ~/.bashrc
root@qrb5165-rb5:/# echo "ROS_IP=`hostname -I`" >> ~/.bashrc
root@qrb5165-rb5:/# echo "ROS_MASTER_URI='http://`hostname -I`:11311'" >> ~/.bashrc
root@qrb5165-rb5:/# source ~/.bashrc

Usage Instructions

Run Moveit! on Qualcomm Robotics RB5 development kit

Note: Before you run the below demo, you should prepare some toys which OpenManipulator robotic arm can pick and place.

On Qualcomm Robotics RB5 Development kit:

root@qrb5165-rb5:/# roslaunch open_manipulator_controller open_manipulator_controller.launch use_moveit:=true

Run RVIZ On ubuntu PC (optional):

Note:please install Moveit on your Ubuntu host PC by referring to this wiki: http://emanual.robotis.com/docs/en/platform/openmanipulator_x/ros_setup/#install-ros-packages

echo "ROS_IP=`hostname -I`" >> ~/.bashrc
source ~/.bashrc
export ROS_MASTER_URI='http://RB5 Device IP address :11311' For example: export ROS_MASTER_URI='http://10.91.4.118 :11311'

Tutorial Video


OpenManipulator with Moveit! on RB5OpenManipulator with Moveit! on RB5

Contributors

NameTitle/Company
Wumei FangSenior, Engineer, Qualcomm Communication Technologies (Shenzhen) Co., Ltd.
Bo ZhangEngineer, Qualcomm Communication Technologies (Shenzhen) Co., Ltd.


Qualcomm Robotics RB5 and Qualcomm QRB6165 are products of Qualcomm Technologies, Inc. and/or its subsidiaries