Integrating CKbot with MoveIt!

Overview

In this tutorial, we will walk through how to use MoveIt on CKbot. We will be able to plan and execute motions on the arms of CKbots via MoveIt!; the following figure shows RViz subscribing to the joint state of the CKbot arm.

_images/IMG_1133.JPG

Prerequisites

First, follow the instructions for installing MoveIt! if you have not already done that. We additionally need two software packages: dynamixel_motor and moveit_plugins. I’m going to grab the whole git directories in src directory of my catkin workspace:

my_catkin_workspace/src $ git clone https://github.com/arebgun/dynamixel_motor.git
my_catkin_workspace/src $ git clone https://github.com/ros-planning/moveit_plugins.git

Then build them:

my_catkin_workspace/src $ cd ..
my_catkin_workspace $ catkin_make

Moving one module

Step 1: Creating a joint controller

In this step, we shall create a joint controller for a single CKbot module. This step has been adapted from the tutorials for dynamixel_motor, installed above, to suit CKbot.

We begin with creating a package which will host launch files:

my_catkin_workspace/src $ roscreate-pkg ckbot_controllers dynamixel_controllers

In my_catkin_workspace/src/ckbot_controllers/launch/, write a launch file, controller_manager.launch, with the following text:

<!-- -*- mode: XML -*- -->

<launch>
  <node name="ckbot_controller_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
    <rosparam>
        namespace: ckbot_manager
        serial_ports:
            port_0:
                port_name: "/dev/ttyUSB0"
                baud_rate: 1000000
                min_motor_id: 1
                max_motor_id: 25
                update_rate: 20
    </rosparam>
  </node>
</launch>

Now connect one u-bar module to a USB serial port; make sure that you have access to the port by running:

$ sudo chown your_user_name /dev/ttyU*

Also make sure (1) you set baud_rate correctly and (2) the id of the module connected should be between min_motor_id and max_motor_id in the launch file.

Now execute the launch file:

my_catkin_workspace/src/ckbot_controllers/launch $ roslaunch controller_manager.launch

We then see the output similar to the following:

[INFO] [WallTime: 1392783197.460336] port_0: Pinging motor IDs 1 through 25...
[INFO] [WallTime: 1392783199.791725] port_0: Found 1 motors - 1 EX-106 [18], initialization complete.

Examine motor feedback by executing:

$ rostopic echo /motor_states/port_0

The output is similar to the following:

---
motor_states:
  -
    timestamp: 1392783349.44
    id: 18
    goal: 2858
    position: 2099
    error: -759
    speed: 0
    load: -0.9658203125
    voltage: 15.7
    temperature: 41
    moving: False

Next, we create a configuration file with parameters needed to actually start controllers. In my_catkin_workspace/src/ckbot_controllers/, write one_ckbot_controller.yaml with the following text:

one_ckbot_position_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: ubar_1
    joint_speed: 5.0
    motor:
        id: 18
        init: 1981
        min: 512
        max: 3450

one_ckbot_joint_trajectory_action_controller:
    controller:
        package: dynamixel_controllers
        module: joint_trajectory_action_controller
        type: JointTrajectoryActionController
    joint_trajectory_action_node:
        min_velocity: 0.25
        constraints:
            goal_time: 1.0

Make sure that the motor id in the 9th row matches the id assigned to the module you connected. (TODO: recheck the encoder tick values.) We now create a launch file that will load the parameters and start the controllers. Paste the following text into my_catkin_workspace/src/ckbot_controllers/launch/start_controller.launch:

<launch>
  <rosparam file="$(find ckbot_controllers)/one_ckbot_controller.yaml" command="load"/>
  <node name="one_ckbot_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
        args="--manager=ckbot_manager
              --port port_0
              --type=simple
              one_ckbot_position_controller"
        output="screen"/>

  <node name="one_ckbot_action_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
        args="--manager=ckbot_manager
              --type=meta
              one_ckbot_joint_trajectory_action_controller
              one_ckbot_position_controller"
        output="screen"/>
</launch>

Now we execute the launch file. Make sure that controller_manager.launch is up and running. Then:

$ roslaunch ckbot_controllers start_controller.launch

Let’s see the topics provided:

$ rostopic list

The output will look like:

$ rostopic list
/diagnostics
/motor_states/port_0
/one_ckbot_joint_trajectory_action_controller/command
/one_ckbot_joint_trajectory_action_controller/follow_joint_trajectory/cancel
/one_ckbot_joint_trajectory_action_controller/follow_joint_trajectory/feedback
/one_ckbot_joint_trajectory_action_controller/follow_joint_trajectory/goal
/one_ckbot_joint_trajectory_action_controller/follow_joint_trajectory/result
/one_ckbot_joint_trajectory_action_controller/follow_joint_trajectory/status
/one_ckbot_joint_trajectory_action_controller/state
/one_ckbot_position_controller/command
/one_ckbot_position_controller/state
/rosout
/rosout_agg

Now we can finally move the robot by publishing a position command in radian on the topic /one_ckbot_position_controller/command:

$ rostopic pub -1 /one_ckbot_position_controller/command std_msgs/Float64 -- 1.5

See the robot moves.

Step 2: Configure MoveIt!

In this step, we shall configure MoveIt! for a CKbot module using the MoveIt! Setup Assistant.

First, create a urdf description file for a CKbot module. For example, write one_ckbot.urdf with the following text:

<?xml version="1.0"?>
<robot name="one_ckbot">

  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.272 0.2634 0.0913"/>
      </geometry>
      <origin xyz="0.0 0.0 0.04565"/>
      <!--The z-coord is the half of the box height.-->
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.272 0.2634 0.0913"/>
      </geometry>
      <origin xyz="0.0 0.0 0.04565"/>
      <!--The z-coord is the half of the box height.-->
    </collision>
  </link>

  <link name="connector_1">
    <visual>
      <geometry>
        <cylinder length="0.00588" radius="0.03"/>
      </geometry>
      <origin xyz="0.0 0.0 0.00294"/>
      <material name="red">
        <color rgba="0.8 0.0 0.0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.00588" radius="0.03"/>
      </geometry>
      <origin xyz="0.0 0.0 0.00294"/>
    </collision>
  </link>

  <joint name="base_to_connector_1" type="fixed">
    <parent link="base_link"/>
    <child link="connector_1"/>
    <origin xyz="0.1072 0.0 0.0913"/>
    <!--The z-coord is the height of the manipulator base.-->
  </joint>

  <link name="bar">
    <visual>
      <geometry>
        <cylinder length="0.0576" radius="0.02"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0288"/>
      <material name="orange">
        <color rgba="0.8 0.4 0.0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.0576" radius="0.02"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0288"/>
    </collision>
  </link>

  <joint name="connector_1_to_bar" type="fixed">
    <parent link="connector_1"/>
    <child link="bar"/>
    <origin xyz="0.0 0.0 0.00588"/>
    <!--The z-coord is the height of the connector.-->
  </joint>

  <link name="rotor">
    <visual>
      <geometry>
        <cylinder length="0.067" radius="0.0327"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="1.57079633 0.0 0.0"/>
      <material name="orange"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.067" radius="0.0327"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="1.57079633 0.0 0.0"/>
    </collision>
  </link>

  <joint name="ubar_1" type="revolute">
    <parent link="bar"/>
    <child link="rotor"/>
    <origin xyz="0.0 0.0 0.0576"/>
    <!--The z-coord is the height of mid_ub_btm.-->
    <axis xyz="0 1 0"/>
    <limit effort="1.0" lower="-1.5708" upper="1.5708" velocity="5.0"/>
  </joint>

  <link name="u_frame">
    <visual>
      <geometry>
        <cylinder length="0.01" radius="0.0327"/>
      </geometry>
      <origin xyz="0.0 0.0 0.005"/>
      <material name="orange"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.01" radius="0.0327"/>
      </geometry>
      <origin xyz="0.0 0.0 0.005"/>
    </collision>
  </link>

  <joint name="rotor_to_u_frame" type="fixed">
    <parent link="rotor"/>
    <child link="u_frame"/>
    <origin xyz="0.0 0.0 0.035"/>
  </joint>

</robot>

In the text, note that the name of the revolute joint, ubar_1, is the same as the joint name appeared in one_ckbot_controller.yaml discussed in Step 1. The urdf model should look like the figure below.

_images/one_ckbot.png

Now execute the MoveIt! Setup Assistant:

$ roslaunch moveit_setup_assistant setup_assistant.launch

In the GUI wizard, load the urdf model and perform the tasks on the left pane: set up Self-Collisions, Virtual Joints, and Planning Groups as explained here. The figures below show how I set up the virtual joint:

_images/vj.png

and the planning group, with the name arm, composed of the revolute joint, ubar_1.

_images/pg.png

We can then skip to generate configuration files. Click on Configuration Files in the left pane; I’ll generate the package in src directory of my catkin workspace:

_images/cf.png

Click Generate Package button as shown above and exit the wizard.

Step 3: Connect MoveIt! and CKbot

In this step, we further configure the MoveIt! package generated in Step 2 to talk to the controllers generated in Step 1.

Navigate to launch directory of the MoveIt! package generated in Step 2 and see the files in there:

my_catkin_workspace/src/one_ckbot_moveit_config $ cd launch
my_catkin_workspace/src/one_ckbot_moveit_config/launch $ ls
default_warehouse_db.launch                     planning_context.launch
demo.launch                                     planning_pipeline.launch.xml
fake_moveit_controller_manager.launch.xml       run_benchmark_ompl.launch
move_group.launch                               sensor_manager.launch.xml
moveit.rviz                                     setup_assistant.launch
moveit_rviz.launch                              trajectory_execution.launch.xml
ompl_planning_pipeline.launch.xml               warehouse.launch
one_ckbot_moveit_controller_manager.launch.xml  warehouse_settings.launch.xml
one_ckbot_moveit_sensor_manager.launch.xml

Modify one_ckbot_moveit_controller_manager.launch.xml such that it looks like the following:

<launch>
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
  <rosparam file="$(find one_ckbot_moveit_config)/config/controllers.yaml"/>
</launch>

In order to load the robot description, change the line in move_group.launch for planning context to match the following:

<include file="$(find one_ckbot_moveit_config)/launch/planning_context.launch">
  <arg name="load_robot_description" value="true"/>
</include>

Create controllers.yaml in my_catkin_workspace/src/one_ckbot_moveit_config/config/ with the following text:

controller_list:
 - name: one_ckbot_joint_trajectory_action_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - ubar_1

Note that joints field above has the joint, ubar_1.

Now create scripts directory in ckbot_controllers package and navigate to the directory. We are going to write a python script that publishes on /joint_states topic such that RViz can visualize the state of the robot; Create ckbot_joint_state_publisher.py with the following text:

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import JointState as JointStateMoveIt
from dynamixel_msgs.msg import JointState as JointStateDynamixel

class JointStateMessage():
    def __init__(self, name, position, velocity, effort):
        self.name = name
        self.position = position
        self.velocity = velocity
        self.effort = effort

class JointStatePublisher():
    def __init__(self):
        rospy.init_node('ckbot_joint_state_publisher')

        rate = 20 # 20Hz
        r = rospy.Rate(rate)

        self.name = ''
        self.current_pos = 0.0
        self.velocity = 0.0
        self.load = 0.0

        # Start controller state subscribers
        rospy.Subscriber('/one_ckbot_position_controller/state', JointStateDynamixel, self.controller_state_handler)

        # Start publisher
        self.joint_states_pub = rospy.Publisher('/joint_states', JointStateMoveIt)

        rospy.loginfo("Publishing joint_state at " + str(rate) + "Hz")

        while not rospy.is_shutdown():
            self.publish_joint_states()
            r.sleep()

    def controller_state_handler(self, msg):
        self.name = msg.name
        self.current_pos = msg.current_pos
        self.velocity = msg.velocity
        self.load = msg.load

    def publish_joint_states(self):
        # Construct message & publish joint states
        msg = JointStateMoveIt()
        msg.name = []
        msg.position = []
        msg.velocity = []
        msg.effort = []

        msg.name.append(self.name)
        msg.position.append(self.current_pos)
        msg.velocity.append(self.velocity)
        msg.effort.append(self.load)

        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'base_link'
        self.joint_states_pub.publish(msg)

if __name__ == '__main__':
    try:
        s = JointStatePublisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

Don’t forget to make it executable:

my_catkin_workspace/src/ckbot_controllers/scripts $ chmod +x ckbot_joint_state_publisher.py

Step 4: Use MoveIt! on CKbot

Now we are ready to use MoveIt! on CKbot. I’ll explain what to execute from the get-go. First, open up a terminal and execute:

$ roslaunch ckbot_controllers controller_manager.launch

Open up a new terminal and execute:

$ roslaunch ckbot_controllers start_controller.launch

Open up a new terminal (or ‌you can stay in the terminal above because start_controller.launch terminates) and execute:

$ rosrun ckbot_controllers ckbot_joint_state_publisher.py

Open up a new terminal and execute:

$ roslaunch one_ckbot_moveit_config move_group.launch

Open up a new terminal and execute:

$ roslaunch one_ckbot_moveit_config moveit_rviz.launch

Set the Planning Scene Topic in MoveIt! Rviz Plugin: /move_group/monitored_planning_scene. Then the virtual CKbot in RViz window should mimic the real CKbot:

_images/IMG_1132.JPG

Make the real CKbot slack, flex it, and see what happens in the window.

On the other hand, start planning using the Planning tab of the MoveIt! Rviz plugin shown below. For example, set your start state and goal state to <current> and <random>; press Plan; if you are satisfied with the plan, press Execute and you should see the real robot moving.

_images/rviz_planning.png

Moving multiple modules

All we need to do ‎is to adapt the parameters and scripts created above (also listed below). Try to do it on your own, but email me (juse@seas.upenn.edu) if you have any trouble.

  1. Check min_motor_id and max_motor_id in controller_manager.launch.
  2. Adapt one_ckbot_controller.yaml.
  3. Adapt start_controller.launch.
  4. Adapt ckbot_joint_state_publisher.py
  5. Adapt the URDF model, one_ckbot.urdf.
  6. Create the MoveIt! package of the new model (name it newbot) and adapt newbot_moveit_controller_manager.launch.xml, move_group.launch, and controllers.yaml.

Misc.

  1. CR modules should be in UBar mode in order to do position control.
  2. It seems that if there is any module that is not working, controller_manager.launch completely fails. To avoid this issue, connect one module at a time, make sure the launch file works, and repeat until you have all the modules found.
  3. In order to publish tf, add the following line to moveit_rviz.launch:
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

Indices and tables