two_arms_sim

A demo to control two UR arms in gazebo via the "FollowJointTrajectiory" action server directly by extracting "pre-recorded" trajectories generated by MoveIt!

Install your dependencies!

Gazebo simulation of two UR arms depend on the universal_robot package, for now. Thus, catkin_make from source (melodic-devel):

https://github.com/ros-industrial/universal_robot.git

For ros MongoDB core, msgs, etc:

sudo apt install ros-melodic-mongodb*

For MoveIt!:

sudo apt install ros-melodic-moveit*

Read this to get an overview first!

Essentially are two parts to making this work.

1. Control two (or more) UR arms in gazebo via the "FollowJointTrajectiory" action server

To do this, we need to:

1. Namespace the UR arms' "FollowJointTrajectory" action server since all arms from universal robot's package are all named "arm_controller/follow_joint_trajectory".

2. Control the arms via the namespaced "FollowJointTrajectory" action server by creating a client to directly send requests.

Scripts/code involved:

1. arm_traj_controller.cpp and arm_traj_controller.hpp

   • a arm_traj_controller class that takes in std::string action_server_name as an argument to create a client to send goal requests to the action server named action_server_name

   • Only has three main working functions. Description can be found in code's comment.

2. Launch files: main.launch <- two_arms.launch <- ur10_arm.launch/ur10_e_arm.launch:

   • ur10_arm.launch/ur10_e_arm.launch is your BASE launch file for launching an arm in gazebo.

   • two_arms.launch is where the namespacing happens.

   • main.launch is where an empty world is generated and the arms are spawned.

2. Storing and extracting trajectories from mongodb:

1. To create a usable db (easy as ABC):

First, create an empty dir anywhere you like.

Then,

roslaunch mongodb_store mongodb_store.launch db_path:=<path_to_your_db_dir>

Done! you can rostopic echo to see if its up.

2. Storing a "pre-generated" trajectory to the db:

First, launch a UR moveit package, you can use universal robot's default pkg:

roslaunch ur10_moveit_config demo.launch

Then, use moveit::planning_interface to plan a trajectory path then just store it to the up and running mongoDB (BE SURE IT"S UP AND RUNNING). You can refer to:

• moveit_example.cpp (which just plans a path then stores it to the up and running mongodb)

• mongodb_extract.cpp (which just extracts/queries the stored trajectory path by name)

Compiling

Remember to compile!

catkin_make

Running!

There are three ros executables in total := moveit_example.cpp (GENERATING TRAJECTORY from moveit & STORING to DB), mongodb_extract.cpp (EXTRACTING TRAJECTORY from DB AND SENDING TO ACTION SERVER) and test.cpp (FOR TESTING SHIT for my own purposes).

1. Generating and storing a trajectory

Launch moveIt! & rviz in terminal A:

roslaunch ur10_moveit_config demo.launch

Then, launch your DB in terminal B:

roslaunch mongodb_store mongodb_store.launch db_path:=<path_to_your_db_dir>

Then run moveit_example.cpp in terminal C:

rosrun two_arms_sim moveit_example

You should see the arm move in Rviz and in the end, the terminal should confirm the storing of a trajectory to your DB.

2. Extracting and sending goal request to action server

Remember to close your previously opened RViz.

Launch the namespaced two arms in gazebo in terminal A:

Gazebo is paused initially, remember to unpause!

If it's not unpaused, action servers won't be up (rostopic to verify namespaced action servers)

roslaunch two_arms_sim main.launch

If you did close it previously, launch your DB in terminal B:

roslaunch mongodb_store mongodb_store.launch db_path:=<path_to_your_db_dir>

Then run mongodb_extract.cpp in terminal C:

rosrun two_arms_sim mongodb_extract

Enjoy the two arms move relentlessly as it hits the ground in sequence!

3.To run test.cpp

test.cpp just sends a manually scripted trajectory path to the arms' action server.

Launch the two arms in gazebo and DB in terminal A and B like before:

roslaunch two_arms_sim main.launch

roslaunch mongodb_store mongodb_store.launch db_path:=<path_to_your_db_dir>

Run the executable in Terminal C,

rosrun two_arms_sim two_arms_sim