KeyframeMotionEngine¶
Keyframe motions are hardcoded interpolations from a start to goal joint configuration, with multiple intermediate steps. They are used for getting up, but also for special postures like goal keeper motions. They can be configured with a handful parameters, to fulfill the need for a simple motion like waving or getting up from the ground even under harsh conditions. The parameters can be found in Config/Robots/Default/keyframeMotionEngine.cfg. Additionally it exist Config/Robots/Default/keyframeMotionParameters.cfg, which contains the parameters for the balancing behavior.
Note
When the robot is trying to get up, you can touch the head sensors to stop the motion. The robot will immediately interpolate into a stand pose. If held upright or standing on the ground, the WalkingEngine takes over, otherwise the robot will start a new get up try.
Control Cycle¶
The main execution cycle is shown in the graphic below. The KeyframeMotionEngine in a classical state machine, to handle different states like deciding which motion to execute, handling breaking up a motion or executing a motion.
The state work is illustrated in more detail below. Here the next requested joint positions are calculated. For it first the joint compensation is updated, for which the current errors of measured and requested positions are determined. Afterwards the start and target positions are interpolated and a simple PID balance controller is applied.
calculateJointDifferences¶
This function calculates the error between the measured joint positions and the requested ones. As there is a delay between when a request is send to the robot and when the effects are measured, we use a buffer of the last requested positions. Normally a buffer of 3 is enough, e.g. the current request has an effect on the measurement 3 frames into the future. Afterwards based on the configuration of the currently executed keyframe a simple linear prediction might be used to further reduce this delay again. Note that the prediction uses the error of the normal subtraction and the prediction, which is closer to the value 0.
In the end we have an error value for each joint.
Note
We actually use a delay of 5 (buffer size of 4 and buffering the request of the previous motion frame). As this code part is from the end of 2018 and was developed for the V5 robots, we never tested whether the correct buffer size would improve the get up routine.
applyJointCompensation¶
Our joint compensation, e.g. the position error correction in case a joint is stuck, is applied to the target request of a given keyframe and not directly applied to the current calculated joint request. We effectively modify the keyframes itself. This can cause some unwanted effects. As an example if the joint hip yaw pitch is requested to change from the position 40 degrees to 0 degrees within one keyframe but is stuck and keeps the position of 40 degrees, we have an error of 40 degrees and the robot is (probably) tilted more forward than inteded and in risk to fall. This error value is multiplied by a factor and added on predefined joints for this specific keyframe. This is not applied directly on the current calculated request of the current motion frame, but on the target request of that keyframe. This means that for example after 10 % execution time of the keyframe only 10 % of the error correction is applied.
This will cause the robot to first fall a bit into the problematic direction. Then the joints start to execute the compensation to catch the robot. This lets the robot swing to the opposite direction and most often resolves the stuck joint. If not the compensation is fully applied at the end of the keyframe and taken over as the start position for the next keyframe to keep the robot upright and prevent a fall.
removePreviouseKeyframeJointCompensation¶
With the current joint compensation we now face the problem that once a new keyframe starts and the problematic joint starts moving again the previous compensation is now overcompensating and will push the robot to the opposite direction. To solve this problem we detect when a compensated joint starts moving and if so we remove all correction from the starting positions of the current keyframe within a few motion frames. In case the joint gets stuck again now a new compensation is automatically added to the current target positions.
Note
This process is far from perfect. On a carpet with lots of ground friction the compensation is hold for so long that at some point the robot will tilt quite fast in the opposite direction. This will unstuck the joint, but now the joints used for compensation can not change their position fast enough. As a result the robot will fall over.
interpolate¶
In the function setNextJoints() after the compensation calculation, a simple interpolation between the start positions and the target positions of the current keyframe is calculated. We use 3 different interpolation types:
- Linear: a typical linear interpolation
- SinusMinToMax: uses the interpolation range of the sinus function from \(-0.5\pi\) to \(0.5\pi\), scaled to the value 0 to 1.
- SinusZeroToMax: uses the interpolation range of the sinus function from \(\pi\) to \(0.5\pi\).
pidWithCom¶
This is our balancer. It uses the center of mass as a control value. Every keyframe has a setpoint defined, which is used to calculate the values for a PID-controller. Additionally we scale the parameters based on the error value, e.g. the error between the setpoint and the measured center of mass. This means the bigger the error the larger the parameters, scaled exponentially.
The resulting value is then applied on predefined joints with an factor. In case a joint is used for balancing in one keyframe and then no longer in the following, the last balancing value will be added on top of the starting position to prevent jumps in the joint positions.
Note
This balancing idea with scaling parameters is from the end of 2018. We never tested if this kind of overengineering is actually necessary. We assume this causes small oscillation in the soles, which in turn reduce the risk of the soles getting stuck in the ground, which would result in joints getting stuck.
Keyframe Parameters¶
An example of a keyframe motion is shown below:
// [...] config entries left out
motions = {
recoverGeneric = {
keyframes = [recover];
balanceOut = false;
keyframeEndRequest = {energySavingLegs = none; energySavingArms = none; isStandHigh = false;};
isMotionStable = false;
startAsNextPhaseMotion = [
{
startAsNextPhase = front;
conditions = [
{
variable = InertialDataAngleY;
range = {min = 0; max = 2000;};
isNot = false;
}
];
isAndCondition = false;
},
{
startAsNextPhase = back;
conditions = [
{
variable = InertialDataAngleY;
range = {min = 0; max = 2000;};
isNot = true;
}
];
isAndCondition = false;
}
];
};
// [...] config entries left out
keyframeBlock = {
stand = {
baseLimbStiffness = [40, 75, 75, 75, 75];
keyframes = [
{
phase = UnknownPhase;
duration = 200;
goalCom = {x = 0; y = 0;};
setLastCom = false;
forbidWaitBreak = false;
interpolationType = Default;
jointCompensation = [
{
jointCompensationParams = [
{
jointDelta = lHipYawPitch;
hipPitchDifferenceCompensation = false;
range = {min = 1deg; max = 10deg;};
jointPairs = [
{joint = lHipPitch; scaling = 1;},
{joint = rHipPitch; scaling = 1;}
];
predictJointDiff = false;
}
];
reduceFactorJointCompensation = 0.2;
}
];
waitConditions = [];
keyframeBranches = [
{
blockID = front;
motionID = [restartFront];
initStiffness = true;
removeCurrentBlock = true;
preCondition = {
conditions = [
{
variable = ShoulderPitchLeft;
range = {min = 50; max = 2000;};
isNot = false;
},
{
variable = ShoulderPitchRight;
range = {min = 50; max = 2000;};
isNot = false;
},
{
variable = ShoulderRollLeft;
range = {min = -2000; max = 20;};
isNot = false;
},
{
variable = ShoulderRollRight;
range = {min = -20; max = 2000;};
isNot = false;
}
];
isAnd = false;
};
useEarlyEntrance = false;
earlyEntranceCondition = {conditions = []; isAnd = true;};
maxNumberOfLoopsBlockID = -1;
maxNumberOfLoopsMotionID = 1;
}
];
balanceWithJoints = {jointY = []; jointX = [];};
singleMotorStiffnessChange = [];
angles = {
head = [0deg, -30deg];
positions = {
leftArm = [-13.9deg, 68.2deg, -122.2deg, -11.1deg, -90deg, 0deg];
rightArm = [-13.5deg, -67.9deg, 121.9deg, 8deg, 90deg, 0deg];
leftLeg = [-3.9deg, -0.2deg, -84.6deg, 123.7deg, -52.9deg, 2.3deg];
rightLeg = [-3.9deg, -0.7deg, -80.2deg, 122deg, -47.9deg, 2.2deg];
};
};
robotPoses = [];
torsoAngleBreakUpEnd = {
pitchDirection = {min = -10deg; max = 2000deg;};
rollDirection = {min = -2000deg; max = 2000deg;};
};
torsoAngleBreakUpStart = [];
},
The configuration file contains two list: motions and keyframeBlock. In motions you can find all higher level motions, while in keyframeBlock you can find motion-parts, which can be recycled for different usages.
Following are some parameters explained for motions:
keyframes: List ofkeyframeBlockmotions. They will be executed one after another.keyframeEndRequest: Flags for the energy saving mode, to prevent overheating.startAsNextPhaseMotion: After this motion is finished, you can set different follow up motions. Like after a goal keeper catch motion, the robot can do a special get up motion if it is still upright.
Following are some parameters explained for the keyframeBlock:
phase: Defines which balancing parameters shall be usedinterpolationType: Defines the kind of interpolation, like linear oder sinus curve based.jointCompensation: Sets parameters for the joint compensation. If a joint in this list gets stuck, the error will be used to apply offsets on the given compensation joints, to keep the robot in a stable position.keyframeBranches: To allow branches in the keyframe execution, like condition extra keyframe or breaking up and starting a completely different motion, you can define ablockID, which is a motion from themotionslist, or amotionID, which is a motion from thekeyframeBlocklist. ifmotionIDis set,blockIDis ignored. The flagremoveCurrentBlockis used, to decide if the current motion shall be removed, or still be executed afterwards.preConditionandearlyEntranceConditionare used as conditional checks, whether the branch shall be executed and if the checks are, then all the time (earlyEntranceCondition) or only at the end of the keyframe interpolation (preCondition).balanceWithJoints: Defines which joints shall be used for the balancing.angles: The target positions of the joints.robotPoses: Currently unused and not fully implemented. If set, replacesangleswith Cartesian space positions, which are converted into target joint positions.torsoAngleBreakUpEnd: Constraints for the torso orientation. If the torso orientation is outside those boundaries, the motion is aborted.torsoAngleBreakUpStart: Start value for the constraints of the torso orientation. Will interpolate to torsoAngleBreakUpEnd over the keyframe duration.
Balancing Parameters¶
An example of the balancing parameters are shown below:
Split = {
balanceFactor = {
maxValAdditionalScaling = {
pitchDirection = {min = 1; max = 3;};
rollDirection = {min = 1; max = 1;};
};
exponentFactorScaling = {
pitchDirection = {min = 2; max = 2;};
rollDirection = {min = 2; max = 2;};
};
pidPPitchParameter = {baseValue = 4; maxAdditionalValue = 5;};
pidPRollParameter = {baseValue = 1; maxAdditionalValue = 0;};
pidDPitchParameter = {baseValue = 1; maxAdditionalValue = 1;};
pidDRollParameter = {baseValue = 1; maxAdditionalValue = 0;};
};
comDiffBase = {
pitchDirection = {min = -50; max = 20;};
rollDirection = {min = -20; max = 60;};
};
};
Following are the parameters explained:
balanceFactor: Contains parameters for the PID-Controller, which is used for the balancing. This controller is separated into forwards/backwards (pitch) and sideways (roll) balancing. Both directions use different balancing values, to compensate with the leg structure and the design of the feet sole, eg. as balancing against a forward tilt is different than against a backward tilt.pid[P|D][Pitch|Roll]Parameter: Base values for the PID-Controller.maxValAdditionalScaling: Max additional scaling for the PID-Controller parameters.exponentFactorScaling: Defines how fast themaxValAdditionalScalingis applied.comDiffBase: The value range for the CoM. The range from 0 to the border values is used to scale the PID-Controller parameters.
Get Up Motions¶
Following are our two get up motions for the front and back shown in slow motion. (Warning: the videos contain flashing lights)
Adding New Keyframe Motions¶
Despite the available motions, you may add further ones to the file following the same structure under a new id. The new id has to be appended of the enumeration KeyframeMotionListID within the file Src/Modules/MotionControl/KeyframeMotionEngine/KeyframeMotionLibs/KeyframePhaseBase.h. Also the new motion must be added in keyframeMotionEngine.cfg. Here it is recommended to copy an existing motion or start SimRobot, load a scene (e.g. BHFast.ros2) and save the parameters with the console with the command save parameters:KeyframeMotionEngine.
To add a new keyframeBlock you need to add the new id in the same file in the enumeration KeyframeMotionBlockID.
If the new motion shall be used as a special motion, like a goal keeper jump, which shall be callable from the behavior, you also need to add an id in the file Src/Representation/MotionControl/KeyframeMotionRequest.h in the enumeration KeyframeMotionID. For ball catching motions, you also need to add a new id in the enumeration Src/Representation/MotionControl/MotionRequest.h Dive::Request and add a conversion in Src/Representation/MotionControl/KeyframeMotionRequest.cpp.
Note
We currently have no tools to design new keyframe motions. For "some" debugging information, you can look into the representation KeyframeMotionGenerator. Feel free to extend this representation.
Current Problems¶
Here we list current problems:
- Some transitions between different keyframe blocks are longer than needed.
- Some conditional branches, like the restart for the front motion, could have better thresholds. They are executed too often.
- The keyframe structure was rebuild to allow more "catch" motions, like when falling backward to use the genuflect or when falling forward to use a front catch motion. Both did not work great and were removed after the first day of the RoboCup 2022. They are planned to be added back in the future.
- The joint compensation is sometimes too much, if the joints used to compensate are stuck. This can result in an overcompensation and the robot falls to the opposite direction.
- This is mostly the case for the forward get up routine. Here the HipYawPitch can get stuck for a very long time and released within in instant. This sudden movement causes the robot to fall over very often.
- The break up thresholds could be improved.
- The fall motion for the forward break up is not healthy for the upper body
- This is a problem since RoboCup 2023. We removed the front catch motion, because it did not work most of the time. Now the arms no longer slow down the fall.