PR2LITE ARM NAVIGATION
We’ve been working on PR2Lite arm navigation for some time. We’ve been building on the work of Pi Robot (Patrick Goebel), Maxwell (Michael Ferguson), Turtlebot(Willow Garage) and Wubble (Anton Rebgun) – all of whom configured ROS to handle dynamixel-based arms.
The issues that are different for PR2Lite include:
- A passive joint formed by a parallelogram for the upper arm that kept the elbow pan joint level with the ground as it is raised or lowered. ROS does not have built in support for URDFs with joints that are not acyclic.
- Having two complex arms
- Having a linear actuator to lift the upper arm. The linear actuator is part of another closed-loop kinematic joint chain.
- Using the ROS Electric Arm trajectory planning code along with a USB2Dynamixel.
We discuss each of these in more detail below. While we’ve made considerable progress, this is still work in progress.
We updated the KDLArmKinematicsPlugin to hardcode our passive elbow joints based on the shoulder angle of the upper arm. Simple changes were made in getRandomConfiguration and searchPositionIK so that ik_seed_state input satisfies our parallelogram constraint. We made our own pluggin: pr2lite_arm_kinematics_constraint_aware that can be found on github:
Once we got the parallelogram supported, the Arm Navigation Warehouse tutorial worked great in simulation. Getting to the Arm Navigation to work on the Real Robot was much harder.
TWO ARMS – NAMING CONVENTIONS
Some of the software that we had dependencies upon had hard-coded naming convensions. Life was much easier when we changed our URDF to match these naming conventions:
– Use the LEFT_ and RIGHT_ prefix for the joint names for the left and right arms respective.
– Use the _JOINT postfix for the joint names.
– Use the _CONTROLLER postfix for the smart-arm controller names.
LINEAR ACTUATOR FOR THE UPPER ARM
Using a linear actuator for the upper arm still looks like a good decision. The linear actuator is inexpensive, with position feedback, and can lift a very heavy load. PR2Lite has 4 Linear actuators (2 for arms, 1 for wheel rotation, 1 for torso lift) and Robert designed a custom controller and firmware.
The robot, the arm and the linear actuator form a triangle with three passive joints. The linear actuator is along the hypoteneus and raises / lowers the arm when the actuator is extended / retracted. However, the passive cyclic joints are a problem for ROS’ urdf. In our URDF, we artificially break the cycle. In software, Nathaniel has added a layer on top of the linear actuator controller that fools ROS into thinking that the linear actuator arm joints is just another dynamixel. This layer does the trig to translate the linear length into an angle of rotation for the upper arm. This greatly simplified the rest of the configuration. Currently, Nathaniel’s controller is being enhanced to return a dynamixel joint state instead of the net485 linear actuator state.
ROS ELECTRIC ARM TRAJECTORY PLANNING
We want PR2Lite to use the ROS Electric arm navigation and trajectory planning, and later we will migrate to the ROS moveit architecture. Unfortunately, there was no perfect precedence for this:
- The turtlebot and Maxwell use the simple-arm driver with Arbotix.
- Wubble uses openRave for the trajectory planning and hacked wrote a custom wubble_follow_joint_trajectory.
- Pi Robot uses OpenRave for the trajectory planning with USB2Dynamixel.
Our configuration uses the following:
- The dynamixel controller by Anton Rebgun (UofA) – which evolved from the simple arm controller.
- The dynamixel joint state publisher from Pi Robot, which maps the dynamixel controller to the joint names and then puts them into an array of joint_states as required by the joint_states message. This required the new naming convention. TODO
- The Arbotix follow_joint_trajectory. This is the key missing piece that maps the ROS electric arm navigation to the dynamixel state / commands. The baud rate and read/write characteristics for the usb2dynamixel have to be set correctly. TBD
- The fake_pos.py from turtle bot. The warehouse viewer will hang if the non-fixed joints in the URDF are not sending out fake positions. To get the list of missing joints, use rxconsole and rxloggerlevel to obtain the DEBUG messages for the environment server.
- A static transform publisher to send out the fixed quaternion for the robot (for the tf from world_joint to the base_footprint). If you see the floating_trans_x/y/z (0,0,0) or floating_rot_x/y/z/w (0,0,0,1) joints, you need either real wheel odometry or the fixed publisher. <node pkg=”tf” type=”static_transform_publisher” name=”quaternion_bc” args=”0 0 0 0 0 1 world_joint base_footprint 100″ />
- planning_scene_warehouse_pr2lite_real.launch required not using sim_time or fake_time; setting use_monitor to true; setting execute_left_trajectory; and setting use_robot_data to true.
Each of the above require their own configuration that needed to be consistent with each other. If everything is not perfect, the warehouse viewer will hang and require debugging using rxgraph, rxconsole, rxloggerlevel, roswtf, the tf tools, etc. It’s not easy and there’s a learning curve. We’re currently modifying the linear actuator code (see above). More changes may be needed, and we’ll blog about these later. Hopefully this mini-tutorial and the github files will help provide some guidance. When all is up, your rxgraph should look something like the following: