sim_controllers_wiki

Mbot simulated controllers

To actuate the mbot sim joints we use ROS control

Our robot has the folowing joints:

• 4 wheel joints: front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel
• 1 neck joint: base_link_to_head_link_joint
• 1 camera joint: head_link_to_head_camera_link
• 7 arm joints: left_arm_joint0, left_arm_joint1, left_arm_joint2, left_arm_joint3, left_arm_joint4, left_arm_joint5, left_arm_joint6
• 1 gripper joint: left_arm_gripper_joint (NOTE: gripper has additional 5 mirror joints)

Base (4 wheel joints)

We use a gazebo plugin borrowed from ridgeback robot, code can be found here and here. Basic idea behind it (is a bit of a hack) because modeling the 4 swedish wheels of the robot is complex, is to transform base commands (/cmd_vel) to a force that is applied to the center of the robot. That explains why is the robot still moving when is lying on the floor with no contact between the wheels and the environment, yet you apply velocites with teleoperation and it moves.

All the code needed for this plugin to work is encoded inside mbot urdf model and in particular here.

There is no joint access control (you can't move individual wheel joints) and the only way to interact with the robot base is trough the topic /cmd_vel and its derivatives:

• /cmd_vel_constant (is /cmd_vel republished for some small amount of time as defined here.
• /cmd_vel_prio_high, /cmd_vel_prio_medium, /cmd_vel_prio_high

NOTE: about mbot_velocity_controller/cmd_vel topic: Do not get confused, this topic we are doing a relay from /cmd_vel to mbot_velocity_controller/cmd_vel (a mirror) because the driver subscribes to it to publish odom data (yes odom data is being generated not from encoders but directly from /cmd_vel), although our odom is not great.. it somehow works. This might need some attention in future.

Neck

It has a position control interface and can be actuated in the same way as in the real robot.

Low level control topic is:

/mbot_head_position_controller/command

of type std_msgs/Float64

As in the real robot we have a high level interface, the semantic head controller, which accepts neck goals in the form of N, S, NE (North, South, North West etc). Check here for more details.

To move the head directly and using the low level interface you can publish directly to the neck command topic:

rostopic pub /mbot_head_position_controller/command std_msgs/Float64 "data: 0.5"

Replace 0.5 by the desired angle according to the following:

Angles are in radians, 0.0 is the head looking front, 1.57 (90 deg) will move the neck to the right (East) and -1.57 (-90 deg) will move the robot neck to the left

Camera joint

The robot does not has a pan tilt unit to move the camera, however we have figured out a way of faking it. By moving the neck and tilting the camera we somehow have a pan tilt unit. This camera joint then, refers to a dynamixel motor connected to the mbot head camera that has the possibility to tilt the camera up and down.

It runs in position control mode and can be actuated in a similar way than the neck:

rostopic pub /mbot_head_camera_position_controller/command std_msgs/Float64 "data: 0.5"

We have created an interface that controls both the real and simulated robot in the same way, so all commands and strategies should work seamlessly. In fact URDF model is shared between real robot and simulated one. This allows for joint limits, efforts, mass, etc to be same.

We then have a republisher that subscribes to the real robot topic and publishes to the gazebo topic, so you can alternatively do:

rostopic pub --once /cmd_head_camera_motor std_msgs/UInt16 75

Arm

The mbot arm has 7 degrees of freedom and can be controlled in the following modes:

• position control
• velocity control
• trajectory control

All controllers are loaded by default and by default the trajectory controller will run at startup. The other controllers (velocity and position) are loaded but stopped. This is because hardware (simulated) resources are claimed by the controllers and cannot support many at the same time (this is true only for simulation). In the case of the real robot we have position and trajectory controller running simultaneously and without conflict, this nice? feature is due to the way the dynamixel driver is designed. In real arm if you send a trajectory and try to also control the arm in position mode, arm will not know what to do (will shake) and the controller publishing at the highest rate will win (typically the position controller wins). This is not advised and please avoid doing it.

You can use a service call to switch between controllers that is provided by the controller manager, however, we have a script to make things easier and an alias.

The command to switch between controllers is:

rosrun mbot_gazebo_control switch_arm_sim_ctrl

and takes as argument the current running controller (i.e. trajectory), and the desired controller (i.e. position or velocity).

Example:

To switch from trajectory controller to position controller do:

rosrun mbot_gazebo_control switch_arm_sim_ctrl trajectory position

To switch back to trajectory controller do:

rosrun mbot_gazebo_control switch_arm_sim_ctrl position trajectory

It is also possible to switch between position and velocity control:

rosrun mbot_gazebo_control switch_arm_sim_ctrl position velocity

NOTE: The above command assumes that your active controller is the position controller.

Alternatively you can visualize all controllers and their states by doing:

rosrun rqt_controller_manager rqt_controller_manager

or

rosservice call /controller_manager/list_controllers "{}"

In order to teleoperate each joint of the arm using the position controller you can run the following interface:

rosrun hacked_joint_state_publisher joint_state_publisher

Gripper

To actuate the gripper we use the gazebo plugin "mimic_joint_plugin" which can be found here

Gripper is actuated by using only 1 joint in position control mode always

To open and close the gripper you can do:

rostopic pub --once /left_arm_gripper/gripper_command mcr_manipulation_msgs/GripperCommand "command: 0"

or use the alias:

open_gripper

To close the gripper:

rostopic pub --once /left_arm_gripper/gripper_command mcr_manipulation_msgs/GripperCommand "command: 1"

or use the alias:

close_gripper