bool KickEngineData::calcJoints(JointRequest& jointRequest, const RobotDimensions& rd, const DamageConfigurationBody& theDamageConfigurationBody)
{
//Calculate Legs
if(motionID > -1)
{
if(!currentParameters.ignoreHead)
{
jointRequest.angles[Joints::headPitch] = head.x();
jointRequest.angles[Joints::headYaw] = head.y();
}
else
{
jointRequest.angles[Joints::headYaw] = JointAngles::ignore;
jointRequest.angles[Joints::headPitch] = JointAngles::ignore;
}
calcLegJoints(Joints::lHipYawPitch, jointRequest, rd, theDamageConfigurationBody);
calcLegJoints(Joints::rHipYawPitch, jointRequest, rd, theDamageConfigurationBody);
simpleCalcArmJoints(Joints::lShoulderPitch, jointRequest, rd, positions[Phase::leftArmTra], positions[Phase::leftHandRot]);
simpleCalcArmJoints(Joints::rShoulderPitch, jointRequest, rd, positions[Phase::rightArmTra], positions[Phase::rightHandRot]);
return true;
}
else //just set the angles from init
{
for(int i = Joints::lShoulderPitch; i < Joints::numOfJoints; ++i)
jointRequest.angles[i] = lastBalancedJointRequest.angles[i];
return false;
}
}
void InverseKinematics::calcLegJoints(const Pose3D& positionLeft, const Pose3D& positionRight, Joints jointAngles, const RobotDimensions& robotDimensions, float ratio)
{
calcLegJoints(positionLeft, jointAngles, true, robotDimensions);
calcLegJoints(positionRight, jointAngles, false, robotDimensions);
Range<float> clipping(0.0f, 1.0f);
ratio = clipping.limit(ratio);
// the hip joints of both legs must be equal, so it is computed as weighted mean and the foot positions are
// recomputed with fixed joint0 and left open foot rotation (as possible failure)
float joint0 = jointAngles[LHipYawPitch] * ratio + jointAngles[RHipYawPitch] * (1 - ratio);
calcLegJoints(positionLeft, jointAngles, joint0, true, robotDimensions);
calcLegJoints(positionRight, jointAngles, joint0, false, robotDimensions);
}
