void InverseKinematics::calcArmJoints(const Vector3<float>& position, const float elbowYaw, Joints jointAngles, bool left, const RobotDimensions& robotDimensions) {
Joint firstJoint(left ? LShoulderPitch : RShoulderPitch);
const int sign(left ? -1 : 1);
const Vector3<float> pos(position - Vector3<float>(robotDimensions.values_[RobotDimensions::armOffset1], robotDimensions.values_[RobotDimensions::armOffset2] * -sign, robotDimensions.values_[RobotDimensions::armOffset3]));
float& joint0 = jointAngles[firstJoint + 0];
float& joint1 = jointAngles[firstJoint + 1];
const float& joint2 = jointAngles[firstJoint + 2] = elbowYaw;
float& joint3 = jointAngles[firstJoint + 3];
// distance of the end effector position to the origin
const float positionAbs = pos.abs();
// the upper and lower arm form a triangle with the air line to the end effector position being the third edge. Elbow angle can be computed using cosine theorem
const float actualUpperArmLength = Vector2<float>(robotDimensions.values_[RobotDimensions::upperArmLength], robotDimensions.values_[RobotDimensions::elbowOffsetY]).abs();
float cosElbow = (sqr(actualUpperArmLength) + sqr(robotDimensions.values_[RobotDimensions::lowerArmLength]) - sqr(positionAbs)) / (2.0f * robotDimensions.values_[RobotDimensions::upperArmLength] * robotDimensions.values_[RobotDimensions::lowerArmLength]);
// clip for the case of unreachable position
cosElbow = Range<float>(-1.0f, 1.0f).limit(cosElbow);
// elbow is streched in zero-position, hence M_PI - innerAngle
joint3 = -(M_PI - acos(cosElbow));
// compute temporary end effector position from known third and fourth joint angle
const Pose3D tempPose = Pose3D(robotDimensions.values_[RobotDimensions::upperArmLength], robotDimensions.values_[RobotDimensions::elbowOffsetY] * -sign, 0).rotateX(joint2 * -sign).rotateZ(joint3 * -sign).translate(robotDimensions.values_[RobotDimensions::lowerArmLength], 0, 0);
/* offset caused by third and fourth joint angle */ /* angle needed to realise y-component of target position */
joint1 = atan2(tempPose.translation.y * sign, tempPose.translation.x) + asin(pos.y / Vector2<float>(tempPose.translation.x, tempPose.translation.y).abs()) * -sign;
// refresh temporary endeffector position with known joint1
const Pose3D tempPose2 = Pose3D().rotateZ(joint1 * -sign).conc(tempPose);
/* first compensate offset from temporary position */ /* angle from target x- and z-component of target position */
joint0 = -atan2(tempPose2.translation.z, tempPose2.translation.x) + atan2(pos.z, pos.x);
}
void BodyContourProvider::update(BodyContour& bodyContour)
{
DECLARE_DEBUG_DRAWING3D("module:BodyContourProvider:contour", "robot");
bodyContour.cameraResolution.x = theCameraInfo.width;
bodyContour.cameraResolution.y = theCameraInfo.height;
bodyContour.lines.clear();
robotCameraMatrixInverted = theRobotCameraMatrix.invert();
add(Pose3D(), torso, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsLeft], shoulder, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsRight], shoulder, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsLeft], upperArm, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::bicepsRight], upperArm, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmLeft], lowerArm, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmRight], lowerArm, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmLeft], lowerArm2, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::foreArmRight], lowerArm2, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighLeft], upperLeg1, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighRight], upperLeg1, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighLeft], upperLeg2, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::thighRight], upperLeg2, -1, bodyContour);
add(theRobotModel.limbs[MassCalibration::footLeft], foot, 1, bodyContour);
add(theRobotModel.limbs[MassCalibration::footRight], foot, -1, bodyContour);
}
/*
* bank in
*/
void BallTaking::phaseOne(BallTakingOutput& output)
{
int t = theFrameInfo.getTimeSince(phaseStart);
float tp = (float)t / phaseDuration;
y = sinUp(tp) * xFactor;
rotX = sinUp(tp) * rotXFactor;
zStand = sinUp(tp) * zStandFactor;
rotZ = sinUp(tp) * rot;
y2 = sinUp(tp) * y2Factor;
if(side > 0) //left swing
{
targetLeftFootPositon = Pose3D(standLeftFoot.translation.x + (y2 * 80), standLeftFoot.translation.y + y + (y2 * 80), standLeftFoot.translation.z + zStand);
targetRightFootPositon = Pose3D(standRightFoot.translation.x, standRightFoot.translation.y + y, standRightFoot.translation.z + zStand);
targetLeftFootPositon.rotateZ(rotZ);
output.angles[JointData::RHipRoll] = -rotX;
output.angles[JointData::LHipRoll] = rotX - y2;
}
else //right swing
{
targetLeftFootPositon = Pose3D(standLeftFoot.translation.x, standLeftFoot.translation.y - y, standLeftFoot.translation.z + zStand);
targetRightFootPositon = Pose3D(standRightFoot.translation.x + (y2 * 80), standRightFoot.translation.y - y - (y2 * 80), standRightFoot.translation.z + zStand);
targetRightFootPositon.rotateZ(-rotZ);
output.angles[JointData::RHipRoll] = rotX - y2;
output.angles[JointData::LHipRoll] = -rotX;
}
if(!InverseKinematic::calcLegJoints(targetLeftFootPositon, targetRightFootPositon, output, theRobotDimensions, 0.5f))
OUTPUT_WARNING("not reachable 1");
//leaving possible ?
if(theFrameInfo.getTimeSince(phaseStart) >= phaseDuration)
{
leftEndPhaseOne = targetLeftFootPositon;
rightEndPhaseOne = targetRightFootPositon;
phaseLeavingPossible = true;
}
}
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
const int sign(left ? -1 : 1);
target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg
target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0
// use cosine theorem and arctan to compute first three joints
float length = target.translation.abs();
float sqrLength = length * length;
float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
float sqrUpperLeg = upperLeg * upperLeg;
float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
float sqrLowerLeg = lowerLeg * lowerLeg;
float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case that target position is not reachable
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg))
{
cosKnee = clipping.limit(cosKnee);
cosUpperLeg = clipping.limit(cosUpperLeg);
reachable = false;
}
float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign;
float joint2 = -acos(cosUpperLeg);
joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z));
float joint3 = M_PI - acos(cosKnee);
RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3);
joint1 -= M_PI_4; // because of the strange hip of Nao
// compute joints from rotation matrix using theorem of euler angles
// see http://www.geometrictools.com/Documentation/EulerAngles.pdf
// this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure
RotationMatrix foot = beforeFoot.invert() * target.rotation;
float joint5 = asin(-foot[2].y) * -sign * -1;
float joint4 = -atan2(foot[2].x, foot[2].z) * -1;
//float failure = atan2(foot[0].y, foot[1].y) * sign;
// set computed joints in jointAngles
jointAngles[firstJoint + 0] = joint0;
jointAngles[firstJoint + 1] = joint1;
jointAngles[firstJoint + 2] = joint2;
jointAngles[firstJoint + 3] = joint3;
jointAngles[firstJoint + 4] = joint4;
jointAngles[firstJoint + 5] = joint5;
return reachable;
}
inline const Pose3D Pose3D::Zero()
{
return Pose3D( Vector3f::Zero(), EulerAnglesf::Zero() );
}
void InertiaSensorFilter::update(OrientationData& orientationData,
const InertiaSensorData& theInertiaSensorData,
const SensorData& theSensorData,
const RobotModel& theRobotModel,
const FrameInfo& theFrameInfo,
const MotionInfo& theMotionInfo,
const WalkingEngineOutput& theWalkingEngineOutput)
{
// MODIFY("module:InertiaSensorFilter:parameters", p);
//
// DECLARE_PLOT("module:InertiaSensorFilter:expectedGyroX");
// DECLARE_PLOT("module:InertiaSensorFilter:gyroX");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedGyroY");
// DECLARE_PLOT("module:InertiaSensorFilter:gyroY");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccX");
// DECLARE_PLOT("module:InertiaSensorFilter:accX");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccY");
// DECLARE_PLOT("module:InertiaSensorFilter:accY");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccZ");
// DECLARE_PLOT("module:InertiaSensorFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
x = State<>();
cov = p.processCov;
lastLeftFoot = lastRightFoot = Pose3D();
lastTime = theFrameInfo.time - (unsigned int)(theFrameInfo.cycleTime * 1000.f);
}
// get foot positions
const Pose3D& leftFoot(theRobotModel.limbs[MassCalibration::footLeft]);
const Pose3D& rightFoot(theRobotModel.limbs[MassCalibration::footRight]);
const Pose3D leftFootInvert(leftFoot.invert());
const Pose3D rightFootInvert(rightFoot.invert());
// calculate rotation and position offset using the robot model (joint data)
const Pose3D leftOffset(lastLeftFoot.translation.z != 0.f ? Pose3D(lastLeftFoot).conc(leftFootInvert) : Pose3D());
const Pose3D rightOffset(lastRightFoot.translation.z != 0.f ? Pose3D(lastRightFoot).conc(rightFootInvert) : Pose3D());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x != 0)
useLeft = theWalkingEngineOutput.speed.translation.x > 0 ?
(leftOffset.translation.x > rightOffset.translation.x) :
(leftOffset.translation.x < rightOffset.translation.x);
else
{
Pose3D left(x.rotation);
Pose3D right(x.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z < right.translation.z;
}
// calculate velocity
Vector3<> calcVelocity, lastCalcVelocity;
float timeScale = 1.f / (float(theFrameInfo.time - lastTime) * 0.001f);
calcVelocity = useLeft ? leftOffset.translation : rightOffset.translation;
calcVelocity *= timeScale * 0.001f; // => m/s
// update the filter
timeScale = float(theFrameInfo.time - lastTime) * 0.001f;
predict(theInertiaSensorData.gyro.x != InertiaSensorData::off ?
RotationMatrix(Vector3<>(theInertiaSensorData.gyro.x * timeScale, theInertiaSensorData.gyro.y * timeScale, 0)) :
(useLeft ? leftOffset.rotation : rightOffset.rotation));
// insert calculated rotation
if(theInertiaSensorData.acc.x != InertiaSensorData::off)
safeRawAngle = Vector2<>(theSensorData.data[SensorData::angleX], theSensorData.data[SensorData::angleY]);
if((theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::sitDownKeeper)) &&
fabs(safeRawAngle.x) < p.calculatedAccLimit.x && fabs(safeRawAngle.y) < p.calculatedAccLimit.y)
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
RotationMatrix calculatedRotation(Vector3<>(
atan2f(usedRotation.c1.z, usedRotation.c2.z),
atan2f(-usedRotation.c0.z, usedRotation.c2.z), 0.f));
Vector3<> accGravOnly(calculatedRotation.c0.z, calculatedRotation.c1.z, calculatedRotation.c2.z);
accGravOnly *= -9.80665f;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
{
if(theInertiaSensorData.acc.x != InertiaSensorData::off)
readingUpdate(theInertiaSensorData.acc);
}
// fill the representation
orientationData.orientation = Vector2<>(
atan2f(x.rotation.c1.z, x.rotation.c2.z),
atan2f(-x.rotation.c0.z, x.rotation.c2.z));
// this removes any kind of z-rotation from internal rotation
if(orientationData.orientation.squareAbs() < 0.04f * 0.04f)
x.rotation = RotationMatrix(Vector3<>(orientationData.orientation.x, orientationData.orientation.y, 0.f));
orientationData.velocity = calcVelocity;
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
// PLOT("module:InertiaSensorFilter:orientationX", orientationData.orientation.x);
// PLOT("module:InertiaSensorFilter:orientationY", orientationData.orientation.y);
// PLOT("module:InertiaSensorFilter:velocityX", orientationData.velocity.x);
// PLOT("module:InertiaSensorFilter:velocityY", orientationData.velocity.y);
// PLOT("module:InertiaSensorFilter:velocityZ", orientationData.velocity.z);
}
void ForwardKinematics::calculateRelativePose(float *joint_angles, float angleXval, float angleYval, Pose3D *rel_parts, float* dimensions) {
Pose3D base = Pose3D(0,0,0)
.rotateX(angleXval)
.rotateY(angleYval);
rel_parts[BodyPart::torso] = base;
rel_parts[BodyPart::neck] = Pose3D(rel_parts[BodyPart::torso])
.translate(0, 0, dimensions[RobotDimensions::torsoToHeadPan])
.rotateZ(joint_angles[HeadYaw]);
rel_parts[BodyPart::head] = Pose3D(rel_parts[BodyPart::neck])
.rotateY(-joint_angles[HeadPitch]);
rel_parts[BodyPart::top_camera] = Pose3D(rel_parts[BodyPart::head])
.translate(dimensions[RobotDimensions::xHeadTiltToTopCamera], 0, dimensions[RobotDimensions::zHeadTiltToTopCamera])
.rotateY(dimensions[RobotDimensions::tiltOffsetToTopCamera] + dimensions[RobotDimensions::headTiltFactorTopCamera] * (M_PI_2 - abs(joint_angles[HeadPan])) / M_PI_2)
.rotateX(dimensions[RobotDimensions::rollOffsetToTopCamera] + dimensions[RobotDimensions::headRollFactorTopCamera] * joint_angles[HeadPan])
.rotateZ(dimensions[RobotDimensions::yawOffsetToTopCamera]);
rel_parts[BodyPart::bottom_camera] = Pose3D(rel_parts[BodyPart::head])
.translate(dimensions[RobotDimensions::xHeadTiltToBottomCamera], 0, dimensions[RobotDimensions::zHeadTiltToBottomCamera])
.rotateY(dimensions[RobotDimensions::tiltOffsetToBottomCamera] + dimensions[RobotDimensions::headTiltFactorBottomCamera] * (M_PI_2 - abs(joint_angles[HeadPan])) / M_PI_2)
.rotateX(dimensions[RobotDimensions::rollOffsetToBottomCamera] + dimensions[RobotDimensions::headRollFactorBottomCamera] * joint_angles[HeadPan])
.rotateZ(dimensions[RobotDimensions::yawOffsetToBottomCamera]);
// Body
for(int side = 0; side < 2; side++)
{
bool left = side == 0;
int sign = left ? -1 : 1;
// Decide on left or right arm/leg
BodyPart::Part pelvis = left ? BodyPart::left_pelvis : BodyPart::right_pelvis;
Joint leg0 = left ? LHipYawPitch : RHipYawPitch;
BodyPart::Part shoulder = left ? BodyPart::left_shoulder : BodyPart::right_shoulder;
Joint arm0 = left ? LShoulderPitch : RShoulderPitch;
//Arms
rel_parts[shoulder + 0] = Pose3D(rel_parts[BodyPart::torso])
.translate(dimensions[RobotDimensions::armOffset1], dimensions[RobotDimensions::armOffset2] * -sign, dimensions[RobotDimensions::armOffset3])
.rotateY(-joint_angles[arm0 + 0]);
rel_parts[shoulder + 1] = Pose3D(rel_parts[shoulder + 0])
.rotateZ(joint_angles[arm0 + 1] * -sign);
rel_parts[shoulder + 2] = Pose3D(rel_parts[shoulder + 1])
.translate(dimensions[RobotDimensions::upperArmLength], dimensions[RobotDimensions::elbowOffsetY], 0)
.rotateX(joint_angles[arm0 + 2] * -sign);
rel_parts[shoulder + 3] = Pose3D(rel_parts[shoulder + 2])
.rotateZ(joint_angles[arm0 + 3] * -sign);
// sbarrett - adding in hand
rel_parts[shoulder + 4] = Pose3D(rel_parts[shoulder + 3])
.translate(dimensions[RobotDimensions::lowerArmLength],0,0);
// Legs
rel_parts[pelvis + 0] = Pose3D(rel_parts[BodyPart::torso])
.rotateX(dimensions[RobotDimensions::torsoHipRoll] * -sign)
.translate(0, 0, -dimensions[RobotDimensions::torsoToHip])
.rotateZ(joint_angles[leg0 + 0] * sign);
rel_parts[pelvis + 1] = Pose3D(rel_parts[pelvis + 0])
.rotateX((joint_angles[leg0 + 1] + dimensions[RobotDimensions::torsoHipRoll]) * sign);
rel_parts[pelvis + 2] = Pose3D(rel_parts[pelvis + 1])
.rotateY(joint_angles[leg0 + 2]);
rel_parts[pelvis + 3] = Pose3D(rel_parts[pelvis + 2])
.translate(0, 0, -dimensions[RobotDimensions::upperLegLength])
.rotateY(joint_angles[leg0 + 3]);
rel_parts[pelvis + 4] = Pose3D(rel_parts[pelvis + 3])
.translate(0, 0, -dimensions[RobotDimensions::lowerLegLength])
.rotateY(joint_angles[leg0 + 4]);
rel_parts[pelvis + 5] = Pose3D(rel_parts[pelvis + 4])
.rotateX(joint_angles[leg0 + 5] * sign);
rel_parts[pelvis + 6] = Pose3D(rel_parts[pelvis + 5])
.translate(0,0, -dimensions[RobotDimensions::footHeight]);
}
//printf("RELATIVE:\n");
//PRINT_PART(rel_parts, BodyPart::right_bottom_foot, "right foot");
//PRINT_PART(rel_parts, BodyPart::left_bottom_foot, "left foot");
//PRINT_PART(rel_parts, BodyPart::torso, "torso");
//PRINT_PART(rel_parts, BodyPart::neck, "neck");
//PRINT_PART(rel_parts, BodyPart::top_camera, "top cam");
//printf("torsoToHeadPan: %2.4f\n", dimensions[RobotDimensions::torsoToHeadPan);
//printf("torsoToHip: %2.4f\n", dimensions[RobotDimensions::torsoToHip);
//printf("torsoToHipZ: %2.4f\n", dimensions[RobotDimensions::torsoToHipZ);
}
