Skip to content

Moving an Arm manipulator

Steve edited this page Sep 14, 2015 · 10 revisions

I have created a little demo of my final result, If you agree with it, We could publish it

Final result GSoC 2015

Please before start you need to set up the ros package, follow this steps to clone and install the ros_neural package.

Mindwave Setup Driver

Run the launch file and be sure pair your bluetooth usb dongle with your Ubuntu system, before run it, turn headset up and change your mac address with the address of your Mindwave mobile headset:

$ roslaunch mindwave_driver bluetooth_start.launch addr:=your_mac_addres_of_mindwave

To move a robot you need to trigger the right signals in your mind to control a robot.

  • Meditation produces alpha waves (frequency range: 7.5-12.5 Hz)
  • attention produces beta waves (frequency range: 12.5-30 Hz)

It will launch and run the node with a topic called /mindwave. This topic contains messages which are being published to see how meditation and concentration values are changing :

$ rostopic echo /mindwave

Moving an Robot Arm Manipulator into Gazebo Simulator

For moving an arm, I have changed the Universal robot arm with the robotiq gripper to pick and place objects. Please install this forked repository.

$ cd catkin_ws/src
$ git clone https://github.com/stonescenter/universal_robot.git
$ git fetch origin ur_robotiq:ur_robotiq
$ git checkout ur_robotiq

Open other terminal and run:

$ roslaunch ur_gazebo ur10.launch limited:=true

Be sure you have launched the driver:

$ roslaunch mindwave_driver bluetooth_start.launch addr:=your_mac_addres_of_mindwave

To start the service node and move the arm with the "mindwave_teleop" package run:

$ roslaunch mindwave_teleop start_demo_arm.launch 

Now you can use the mindwave headset to move the arm over a predefined trajectory to open/close the hand, raising more higher concentration and meditation than threshold values in start_demo_arm.launch file :

<launch>
  <!-- the first trajectory goal-->
  <arg name="pick" default="pick.csv" />
  <!-- the second trajectory goal -->
  <arg name="place" default="place.csv" />

  <!-- Start service node of execution of trajectories -->
  <include file="$(find mindwave_execute_trajectory)/launch/start_service.launch"/>

  <node pkg ="mindwave_teleop" type="reproduce_trajectory_node" name="reproduce_trajectory_node"
        output="screen">
	    <param name="attention_threshold" value="40" type="int"/>
	   	<param name="meditation_threshold" value="65" type="int"/>
	   	<param name="pick" type="string" value="$(arg pick)" />
	   	<param name="place" type="string" value="$(arg place)" />
  </node>

</launch>


Changing trajectories

If you want to change the default trajectories pick.csv or place.csv declared at mindwave_teleop/launch/start_demo_arm.launch file. I have created a litle script to save bag files and convert to a set of points .csv

You need to use Moveit to plan your new trajectory. The way that I am getting this trajectories:

$ roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch  sim:=true limited:=true

Open other terminal:

$ roslaunch ur10_mooveit_config moveit_rviz.launch config:=true

Now, you need to run this launch while moveit plans/executes the trajectory to save in bags files under "mindwave_execute_trajectory/bags" directory, open other terminal:

$ roslaunch mindwave_teleop follow_trajectory.launch 

It will save a bag file of the jointstates of your robot arm, then you need to convert to set of points(if you don't like my trajectory don't worry you can generate your own trajectory)

$ python scripts/read_bagfile.py bags/trajectory_your_date.bag

It will generate something similar to "output-join-states.csv" in the current directory. This is the new trajectory that you can replace the rosparam with the new place.csv , it just needs to do once time.

<launch>
  <!-- the first trajectory goal-->
  <arg name="pick" default="pick.csv" />
  <!-- the second trajectory goal -->
  <arg name="place" default="place.csv" />

  <!-- Start service node of execution of trajectories -->
  <include file="$(find mindwave_execute_trajectory)/launch/start_service.launch"/>
   .
   .
   .
</launch>

With all these information you can use the mindwave to start moving the arm to open/close the hand raising the correct concentration and meditation values.