Motion
- Literature
- Naosim
- Overview
- Actions
- Kinematics
- Calibration
- Walk2014 Generator
- Kicks
- Walk Preprocessor
- SafetySkill
The most relevant report to motion currently is: rUNSWift Walk2014 Report (Hengst, 2014)
Other reports that include motion concepts to varying degrees:
- Learning to Control a Biped with Feet (Hengst, Lange, White, 2011)
- Humanoid Omni-directional Locomotion (White, 2011)
- Dynamic Omnidirectional Kicks on Humanoid Robots (Teh, 2012)
- Reinforcement Learning of Bipedal Lateral Behaviour and Stability Control with Ankle-Roll Activation (Hengst, 2013)
- Bipedal walk and goalie behaviour in Robocup SPL (Liu, 2013)
- rUNSWift Robocup SPL 2013 Special Project A Report (Padilha, 2013)
Naosim is a simulator developed by Bernhard Hengst that presents a physically accurate model of nao in a simulated environment. The physics of both the nao and this environment can be interacted with for purposes of testing and observation. Installation instructions can be found at Naosim installation.
The motion module is run in its own thread with the highest priority on the robot, as joint angles need to be calculated at 100 frames per second to maintain stability. The MotionAdapter class controls this thread and is the gateway for communicating in and out of the motion module. Within motion, there are 3 main areas.
Touch: receives and filters input from the Nao's sensors, eg gyroscopes, accelerometers, etc
Generator: processes the behaviour request and sensor input to generate appropriate joint values and subsequent odometry
Effector: uses generated joint values to output robot action
Most of the work in producing actions happens in the generator area. The main generator class is the DistributedGenerator that calls upon subsequent generators depending on the requested action. There are also some basic generators that all generated joints get passed through, such as the ClippedGenerator, which ensures that joint requests do not go past their maximum values.
All actions are defined in $RUNSWIFT_CHECKOUT_DIR/robot/types/ActionCommand.hpp. If you do add a new ActionType to it, remember to fill out all the corresponding enums, such as the action's priority. These priorities define what actions can be interrupted, for example the get up action has the highest priority.
To set a generator for a particular body action, you'll need to add something like this to DistributedGenerator:
bodyGenerators[Body::WALK] = (Generator*)(new PlannedWalkGenerator());
if (!bodyGenerators[Body::WALK])
llog(FATAL) << "bodyGenerators[WALK] is NULL!" << std::endl;All generators should subclass the basic Generator.hpp class and fill in the virtual functions, such as isActive, reset, and stop. Along with the ActionCommand priorities, these functions help DistributedGenerator decide how and when to transition between different actions.
Static actions are typically used for straightforward motions such as standing and squatting. These are handled by the ActionGenerator. To specify which action should be performed, the ActionGenerator is passed the file name of the relevant .pos file.
Our all pos files can be found in $RUNSWIFT_CHECKOUT_DIR/image/home/nao/data/pos. They typically look something like this, for example stand.pos:
# HY HP LSP LSR LEY LER LWY LH LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY RH DUR
0 0 90 10 0 0 0 0 0 0 -28 50 -25 0 0 -28 50 -25 0 90 -10 0 0 0 0 1000
$ 0 0 0 0 0 0 0 0 0.75 0.75 0.75 0.75 0.75 0.75 0.75 0.75 0.75 0.75 0.75 0 0 0 0 0 0
The first line typically starts with a comment to remember the order in which joint values specified. The following lines then specify the joint values for each of those joints in degrees. For the range limit for each joint, check utils/body.hpp or the Aldebaran documentation. (If they differ, please update body.hpp).
By default the stiffness of each of the joints is set to 1. However, if desired, each joint values line can be followed by a stiffness line to change the power of the motors. Stiffness settings range from 0 to 1, where 0 is no power to the motor and 1 is full power to the motor. Stiffness can also be set to -1 to turn the motor off.
#: the rest of the line is a comment
HY HP LSP etc: the name of a joint, eg Head Pitch, Head Yaw, etc
DUR: duration of the interpolation to the specified joint values in milliseconds. not applicable in stiffness lines.
$: this line specifies stiffness. $ needs to be at the very start of the line.
Note: Joint values in the .pos files are specified in degrees for convenience. However the hand joint (LH/RH) requires an input of either 0 or 1 radian to close or open the hand respectively. Thus be sure to specify the degrees equivalent if opening the hand in a pos file (ie 57.295...).
Note/Troubleshooting: Be sure to specify a joint value for each of the joints, or stiffness value if stiffness is specified. Our pos file parsing code is extremely finicky and requires that every cell be filled appropriately. If not, rUNSWift will not run. You should receive an error message, but otherwise you may experience symptoms such as your robot dying and dropping off the network as soon as you try to start runswift on it. If this happens please look into why the error messaging isn't working :)
Dynamic actions are for more complicated movements, such as the walk and kicks. Instead of reading the joint values from a pos file, they are set in the relevant generator's makeJoints function.
To add a new action into the Python code, you'll need to add to the python wrapping of ActionCommand in $RUNSWIFT_CHECKOUT_DIR/robot/perception/behaviour/python/wrappers/ActionCommand_wrap.cpp. (See line 66 for ActionTypes)
Then add it to $RUNSWIFT_CHECKOUT_DIR/image/home/nao/data/behaviours/actioncommand.py. For example here is the one for walk on line 12:
def walk(forward=0, left=0, turn=0, bend=math.radians(15), speed=1.0, isFast=False):
return robot.BodyCommand(robot.ActionType.WALK, int(forward), int(left), float(turn), 0.0, bend, speed, 0.0, robot.Foot.LEFT, isFast)You will then be able to call your new action from a Python behaviour like so:
request.actions.body = actioncommand.walk(300, 0, turn)The motion module will know which behaviour is running as MotionAdapter.cpp will read it from the blackboard, and then call upon the appropriate generator.
// Get the motion request from behaviours
int behaviourReadBuf = readFrom(behaviour, readBuf);
ActionCommand::All request = readFrom(behaviour, request[behaviourReadBuf]).actions;
...
...
...
// Get the joints requested by whichever generator we're using
JointValues joints = generator->makeJoints(&request, &odo, sensors, bodyModel, ball.x(), ball.y());These can be found in perception (just a legacy location as it was originally only used for vision).
DH parameters are used to calculate the forward kinematics chain. See here for the Camera to Foot transform used in the Nao v3 (most of which is similar in the Nao v4) These are used for calculating the robot pose and converting camera image space co-ordinates to robot relative co-ordinates.
The DH parameters for each of Nao v4 limbs can be found here (see Tables 3.1-3.3). These are used for calculating the centre of mass.
Note that body lean (as detected from IMU) is also applied when converting into world space as the robot's foot is not always flat on the ground.
Both Kinematic Transforms and Iterative Inverse Kinematics for turning the feet can be found at the very bottom of Walk2014.cpp. They were copied from the symbolic Matlab solution and of the kinematic chain and iterative inverse method. Reference to the original work can be found in the [2010 team report] (http://cgi.cse.unsw.edu.au/%7Erobocup/2010site/reports/report2010.pdf), Chapter 6.
Please see the Kinematics Calibration page.
Walk2014 addresses shortcomings in the walk first developed in 2010, i.e.
- Stability - addressed by a new Stabiliser using a reinforcement learnt policy and the Centre-of-Mass moved towards centre of foot.
- Slow side-stepping - addressed with a new sidestepping generator that can take larger sidesteps.
- Robots overheat - addressed by using a stand command with low stiffness.
- Transition between stand, walks, kicks, and getup is not smooth - addressed by integrating these behaviour into the walk.
- Acceleration and braking uses an inefficient ratcheting technique - addressed by ability to start and stop in one step. Forward is still limited to changes of e.g. 50mm / step, (built into walk).
- Limited repertoire of kicks - addressed by new kicks, e.g. stab-kick, walk-kick, etc.
Walk2014 is more responsive than previous rUNSWift walks. Python roles and skills have more control (and hence responsibility) for parameter settings:
- The forward, left and turn body action commands define the step-size per second.
- The user will need to ensure that the combination of forward/left/turn stays within the capability of the walk. However, the values have been clipped to +/- 300mm for forward, +/-200mm for left, and +/- 1.5 radian for turn. Please note that the above ranges for forward, left, turn are with only one parameter set, the others are zero). In combination these ranges will need to be reduced, but combinations have not all been tested. Because it is complex to predict the combination of parameter values and transitions that work and may be useful to the situation, the idea is to leave this to behaviour at this stage. It is possible to scale back all values in proportion using a simplex shape. Here is the code to use in behaviour (Python):
def simplexScaleWalk2014(forward, left, turn):
sum = abs(forward/300) + abs(left/200) + abs(turn/1.5)
if sum <= 1 :
return forward, left, turn
return round(forward/sum), round(left/sum), turn/sum... but is likely not to be optimal for the behaviour intended
With actioncommand.stand(power = stiffness) the walk performs a stand routine and turns the stiffness to the motors using power. The power parameter is optional and defaults to 0.1.
actioncommand.crouch(power = stiffness), the walk stops rocking, but is READY for action i.e. legs are kept bent. Stiffness is kep to 0.4 minimum to ensure the robot retains this stance. The power parameter is optional and defaults to 0.4.
However, there are some cases where the robot needs to stand but walk shouldn't be running, for example when the robot first boots up or during ref pick up. We then use two pos files (initial.pos and stand.pos) that map to these two different stand postures. Finally, we need to map the joint positions in initial.pos in libagent, since that runs before our rUNSWift code can run. See stand_angles in $RUNSWIFT_CHECKOUT_DIR/robot/libagent/libagent.cpp
The walk has been calibrated so that forward, left and turn will move the real robot approximately the specified distance/angle in one second in steady state operation. The odometry is calibrated based on foot movements and should be more accurate than average parameter values. For example, when changing speed, the odometry will record the movements at 100Hz and not use the parameter values.
The Jab-Kick is a forward jab with either the left or right foot while it is the swing-foot. The robot must be walking for the kick to activate. It is intended to be used while pacing on the spot and will only be effective if the ball is very close to the kick-foot. To invoke use actioncommand.jabKick(forward, left, turn, power, foot). Default is actioncommand.jabKick() which sets forward = 1, left = 0, turn = 0, power = .5, foot = body.Foot.LEFT. Power is size of jab where 1 represents the maximum accepted forward foot swing (i.e. walking at 300mm/sec).
The standard kick is also integrated into Walk2014Generator to make transitioning between the two as smooth as possible. It uses many of the same parameters and calculations. For example forward left and turn are still used to define the step size, but over the length of the kick period.
The motion of the kick is defined in several phases:
-
The back phase moves the foot backwards in preparation for the kick swing. It also moves the foot sideways depending on the position of the ball. This dynamic left movement helps reduce time spent lining up.
-
The kick swing phase swings the foot forward into the ball. The power parameter is used at this stage to affect the period of the kick swing phase. A lower power increases the period, thereby slowing down the kick foot and reducing the distance travelled by the ball.
-
The follow through phase holds and stabilises at the end of the kick swing.
-
The end phase returns the foot back to the zero position.
Since the behaviour module runs at 30fps in a separate thread and does not have access to the state of the walk cycle found in walk generator, it has no precise way to tell when a step begins and when it ends. That's why walk parameters are given as step size per second. However, this makes certain higher level behaviours difficult when precision of steps is required, such as lining up to the ball before kicking it.
The walk preprocessor is a wrapper around the walk generator and has access to the state of the walk cycle. It breaks down a single higher level behaviour request into multiple walk parameters for the walk generator. Thus avoiding the need for synchronisation between the behaviour and motion thread, and ensures precision of steps.
The walk preprocessor handles lining up to the ball before kicking. The behaviour module calls kick when close enough to the ball. The walk preprocessor interprets the kick command and determines whether the kicking foot is within threshold for the kick. If not, walk commands are given to the walk generator and exact steps can be taken to lineup the kicking foot to the ball. Once the foot is lined-up, the kick command is executed. Fluid transition between walking and kicking can be achieved since it's all done in one high level behaviour request and the kick module is within the walk generator as well.
actioncommand.kick(foot=kickingFoot, turn=relativeKickHeading)There are 2 notable input parameters for a standard kick command.
- foot: which foot to kick with.
- turn: the target heading relative to the robot you want to kick towards.
The kick command can be given at any distance to the ball. But since the obstacle avoidance behaviour is required when closing in, and is not yet available once the kick command is called, we should only call kick when close enough to the ball. E.g. 300mm.
During the lineup phase of the kick, the walk preprocessor can still be interrupted with other commands. For example if the ball was stolen during the lineup phase, walk commands can be given to change directions or open up other options.
Turn dribble is a dribbling technique that consists of multiple walk commands. In competition, turn dribble is used when trying to steal the ball from another close robot.
Turn dribble to the left consists of:
- step close to ball and align the inside of right foot to the ball
- turn left foot out roughly 45 degrees.
- follow through with the right foot forward, hitting the ball to the left.
The ball should be dribbled to the side and the robot should be facing that direction too. The idea is to enable faster follow up actions after the steal was successful.
The actioncommand interface and the lineup requirements are similar to the standard kick.
WalkKickDribble.py is a good example on how the behaviour to motion interface work.