OpenManipulator with Moveit!

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

• Qualcomm Robotics RB5 development Kit
• OpenCR
• OpenMANIPULATOR
• MoveIt

Source Code / Source Examples / Application Executable

• DynamixelSDK
• Dynamixel workbench
• Dynamixel workbench message
• Openmanipulator
• Openmanipulator message
• Openmanipulator simulations
• Robotis manipulator

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 Qualcomm 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'

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