Animating Robot Limbs with ROS: Atlas Climbs
This is my final project working in a team of two with Brit Wylie for ME133a, a course at Caltech on robotic systems. The course focuses on the kinematics and inverse kinematics of multi-DOF systems such as robotic arms and also served as my introduction to ROS.
Our team decided to animate the Atlas URDF climbing a ladder in Rviz. We generated and encoded the ladder URDF as a function inside of the Atlas URDF, and we modified some of the Rviz and launch files to program (some different types of) motion into the robot.
In this project, we had to consider how to animate Atlas with respect to a fixed ground and how to solve the motion of the limbs such that Atlas realistically moved limbs from one rung to another. We also had to handle additional degrees of freedom, stopping the limbs from colliding with the ladder, and keeping the limbs that were not moving stationary with respect to ground.
Robot and Task Description
Robot
This system takes advantage of the class-provided atlas_description package and atlas_v5.urdf. The URDF of Atlas itself already contains 30 degrees of freedom, far more than necessary for our task. There are 6 DOFs for each leg to the pelvis, 7 DOFs for each arm to the shoulder, 2 DOFs for the neck, and 2 shared DOFs for the back. In the simplest iteration, we fix the location of the two back joints and the two neck joints.
For our project Atlas has a moving base, so we add a world link to the system as: <link name="world"/> Every turn, we must solve all joints contained within Atlas against each other, and transform these joint positions in the world frame.
We also update the urdf to include the ladder information. This is stationary with respect to the world and the number of rungs are initialized once, at the start of the visualization. The ladder does not move. These rung positions have been set by trial and error in the Rviz system to avoid fully extending the limbs and having to handle more singularities and collisions.
Atlas initialized in “world” coordinate frame
Task
In order to animate Atlas moving vertically up the ladder, we must fix three joints at a time with respect to the world and move the fourth in a prescribed motion from one rung to the next.
At the suggestion of the advisors, in the simplest version of the animation we set the pelvis of Atlas to move up with constant v and then solve all joints with respect to the pelvis. This means that the “fixed” limbs will be moving with a constant -v, opposite the direction of motion of the pelvis.
We care about the position and orientation of each limb at a time, so with all joints fixed this leaves us with 30 - 6*4 = 30 - 24 = 6 free degrees of motion. The head and neck do not affect the position of the body, so we discount 2 of the degrees. Then there are 4 additional degrees of motion in the system. We fix the back joints, removing 2 more degrees of motion. Then the remaining two extra degrees are split between the arms.
The overall procedure for setting up the animation is as follows:
-
Initialize all joints at positions that orient the limbs approximately how they look while climbing, taking care to avoid singularities (e.g. perfectly straight legs)
-
Move all joints to starting positions on the ladder (1s)
-
Move joints in order
-
Compute locations relative to pelvis
-
Tf2 broadcaster sets motion of pelvis upwards
-
-
Repeat until Atlas reaches the top of the ladder
-
End animation/cycle back to the beginning
Algorithm and Implementation
Algorithm Summary
We animate Atlas’s limbs relative to Atlas’s pelvis, i.e. in “Atlas’s frame.”
Let:
rung = distance between rungs
T = time for Atlas to travel upward 1 rung
v_atlas = velocity of Atlas’ pelvis relative to world = rung / T
The figure above depicts the relative positions of Atlas’s feet relative to the world frame (in which the ladder is static, and Atlas is moving upwards with velocity v_atlas), and relative to Atlas’s frame (in which the ladder is moving downwards with velocity -v_atlas, and Atlas is static). Note that in one period T, in Atlas’s frame, Atlas’s left foot moves down a distance of rung and his right foot moves upwards a distance of rung. Then the roles of the left and right feet switch. The arms behave in an identical manner, and we decided to have Atlas climb as a human would with diagonally opposite limbs moving rungs simultaneously.
We employ the following terminology. “Active foot/hand” describes the hand/foot moving to a new rung; “static foot/hand” describes the foot/hand standing/holding on a rung. Between each period T, the right hand/left foot and left hand/right foot alternate between active and static roles.
-
When static, the foot/hand moves (relative to Atlas’s frame) down a distance of rung over period T at a constant rate of -v_atlas.
-
When active, the foot/hand moves (relative to Atlas’s frame) simultaneously away from the ladder in the negative x directions, a net distance of rung up, and back towards the ladder in the positive x direction over period T via the following formulas, where a = max x-distance foot gets from ladder. Note that the parabola defined for the foot’s z-equation has a max slightly higher than 1, which is achieved slightly before s = 1. The parabola thus animates the foot stepping down onto the step of the ladder. The table below shows the path equations. A cubic spline is then solved for s0 = 0, sf = 1, and v0 = vf = -v_atlas to get the trajectory of the hand/foot in task space. The rotation of the hands and feet is set to a constant orientation.
Inverse kinematics are solved using the Jacobian inverse method, with a rate of convergence λ = 10. Note that although we used the pseudoinverse, we could have used the regular inverse - we had 6DOF desired (xyz and rpy), and effectively 6DOF available system for each limb:
-
Leg: hip xyz (3) + knee (1) + ankle xy (2) = 6
-
Arm: shoulder xz (2) + elbow xy (2) + wrist xy* (2) = 6. Note that the wrist actually had 3DOF, wrist x, wrist y, and wrist y2. However, wrist y and wrist y2 were completely redundant, being right next to each other and about the same axis. So the supposed 7DOF arms were not so.
Avoiding singularities and rung-limb collisions
Any singularities are mainly avoided by setting the starting positions of the joints correctly. The rungs are spaced at a “reasonable” distance in order for Atlas to avoid singularities.
Collisions are avoided via the paths and task-space trajectories of the feet. (The arms are less of a concern, as the hands are animated as “sticking” to the sides of ladder rungs rather than stepping atop them). An earlier iteration of the code used the same xyz trajectories for the arms and legs (see algorithm summary), which caused the foot to slide through the body of the rung. This was fixed with the parabolic foot trajectory, overshoots the z-trajectory above the step and then sets the foot back down.
Left: before implementing the parabolic tractor, foot glides through ladder rung
Right: after implementing the parabolic trajectory, foot steps down onto the ladder rung
Coordinating multiple kinematic chains
To coordinate four kinematic chains, we put the limb trajectory and ikin (inverse kinematic) code in four separate python scripts:
-
move_l_leg.py
-
move_r_leg.py
-
move_r_arm.py, and
-
move_l_arm.py
The main python script, atlas_climbs.py, imports the limb scripts and uses their classes to create an object for each limb. The __init__() functions each take inputs of rung and T, which are used to determine trajectories of each limb - so it only takes changing one line of code to change how far apart the ladder rungs are spaced (plus one edit in the URDF) or how fast Atlas climbs. In each limb, the update() function outputs two lists - one that is a list of joint names, and one that is a list of joining values. Finally, the main script concatenates these lists, along with some constant values for redundant joints, and sends the joint state message.
Plotting joint-states
Looking at a joint plot for our robot (legs shown, similar plots for arms), we can see that our motion is dominated mainly by 3-4 joints on each limb, with others coming in to adjust as needed.
