void KickModule::commandLegsRelativeToTorso(float *command_angles, Pose3D left_target, Pose3D right_target, float /*tilt*/, float roll, float /*tilt_roll_factor*/, bool left_compliant, bool right_compliant) {
RotationMatrix rot;
RotationMatrix foot_rotation;
rot.rotateX(roll);
foot_rotation.rotateX(-roll);
if (left_compliant) {
left_target.translation = rot * left_target.translation;
left_target.rotation = left_target.rotation * foot_rotation;
}
if (right_compliant) {
right_target.translation = rot * right_target.translation;
right_target.rotation = right_target.rotation * foot_rotation;
}
bool isValid = false;
//cout << "*** command legs relative to torso " << endl;
//cout << "commandLegsRel() left foot x y z " << left_target.translation.x << ", " << left_target.translation.y << ", " << left_target.translation.z << endl;
//cout << "left foot rotation x y z " << left_target.rotation.getXAngle() << ", " << left_target.rotation.getYAngle() << ", " << left_target.rotation.getZAngle() << endl;
//cout << "commandLegsRel() right foot x y z " << right_target.translation.x << ", " << right_target.translation.y << ", " << right_target.translation.z << endl;
//cout << "right foot rotation x y z " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << endl;
isValid = inverse_kinematics_.calcLegJoints(left_target, right_target, command_angles, robot_info_->dimensions_);
command_angles[LHipYawPitch] = 0.0;
command_angles[RHipYawPitch] = 0.0;
}
RotationMatrix ForwardKinematics::calcChestFeetRotation(const KinematicChain& theKinematicChain)
{
RotationMatrix calculatedRotation;
// calculate rotation based on foot - torso transformation
const Pose3D& footLeft = theKinematicChain.theLinks[KinematicChain::LFoot].M;
const Pose3D& footRight = theKinematicChain.theLinks[KinematicChain::RFoot].M;
const Pose3D& body = theKinematicChain.theLinks[KinematicChain::Torso].M;
// local in chest
Pose3D localFootLeft(body.local(footLeft));
Pose3D localFootRight(body.local(footRight));
if(abs(localFootLeft.translation.z - localFootRight.translation.z) < 3.f/* magic number */)
{
// use average of the calculated rotation of each leg
double meanX = (localFootLeft.rotation.getXAngle() + localFootRight.rotation.getXAngle())*0.5;
double meanY = (localFootLeft.rotation.getYAngle() + localFootRight.rotation.getYAngle())*0.5;
//calculatedRotation.fromKardanRPY(0.0, meanY, meanX);
calculatedRotation.rotateX(meanX).rotateY(meanY);
}
else if(localFootLeft.translation.z > localFootRight.translation.z)
{
// use left foot
calculatedRotation = localFootLeft.rotation;
}
else
{
// use right foot
calculatedRotation = localFootRight.rotation;
}
return calculatedRotation.invert();
}//end calcChestFeetRotation
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
int sign(left ? -1 : 1);
target.translation.y += (float) robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2.f * sign; // translate to origin of leg
// rotate by 45° around origin for Nao
// calculating sqrtf(2) is faster than calculating the resp. rotation matrix with getRotationX()
static const float sqrt2_2 = sqrtf(2.0f) * 0.5f;
RotationMatrix rotationX_pi_4 = RotationMatrix(Vector3<float>(1, 0, 0), Vector3<float>(0, sqrt2_2, sqrt2_2 * sign), Vector3<float>(0, sqrt2_2 * -sign, sqrt2_2));
target.translation = rotationX_pi_4 * target.translation;
target.rotation = rotationX_pi_4 * target.rotation;
target = target.invert(); // invert pose to get position of body relative to foot
// use geometrical solution to compute last 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 cosLowerLeg = (sqrLowerLeg + sqrLength - sqrUpperLeg) / (2 * lowerLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case of unreachable position
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(cosLowerLeg))
{
cosKnee = clipping.limit(cosKnee);
cosLowerLeg = clipping.limit(cosLowerLeg);
reachable = false;
}
float joint3 = M_PI - acosf(cosKnee); // implicitly solve discrete ambiguousness (knee always moves forward)
float joint4 = -acosf(cosLowerLeg);
joint4 -= atan2f(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs());
float joint5 = atan2f(target.translation.y, target.translation.z) * sign;
// calulate rotation matrix before hip joints
RotationMatrix hipFromFoot;
hipFromFoot.rotateX(joint5 * -sign);
hipFromFoot.rotateY(-joint4 - joint3);
// compute rotation matrix for hip from rotation before hip and desired rotation
RotationMatrix hip = hipFromFoot.invert() * target.rotation;
// 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 (z, x, y seen from body resp. y, x, z seen from foot)
float joint1 = asinf(-hip[2].y) * -sign;
joint1 -= M_PI_4; // because of the 45°-rotational construction of the Nao legs
float joint2 = -atan2f(hip[2].x, hip[2].z);
float joint0 = atan2f(hip[0].y, hip[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;
}
// TODO: Set the other camera matrix's isValid to false if use current camera
void ODECameraProvider::update(CameraMatrix* theCameraMatrix)
{
if(theCameraConfig->usingTopCam)
{
int supfootLink = 0;
RotationMatrix RHeadW;
RotationMatrix RobotRotationW;
RobotRotationW.rotateX(theNaoStructure->supportFootRotWorld.x).rotateY(theNaoStructure->supportFootRotWorld.y).rotateZ(theNaoStructure->supportFootRotWorld.z);
if(theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_LEFT || theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
supfootLink = NaoStructure::lFootLink;
}else
{
supfootLink = NaoStructure::rFootLink;
}
//RotationMatrix test = theNaoStructure->uLink[supfootLink].R.invert();
RHeadW = /*RobotRotationW * */(theNaoStructure->uLink[NaoStructure::headEndLink].R * theNaoStructure->uLink[supfootLink].R.invert());
//double rx = RHeadW.getXAngle();
//double ry = RHeadW.getYAngle();
//double rz = RHeadW.getZAngle();
//rx = theNaoStructure->uLink[NaoStructure::headEndLink].R.getXAngle();
//ry = theNaoStructure->uLink[NaoStructure::headEndLink].R.getYAngle();
//rz = theNaoStructure->uLink[NaoStructure::headEndLink].R.getZAngle();
theCameraMatrix->translation = RHeadW * Vector3<double>(CameraTopOffsetX, CameraTopOffsetY, CameraTopOffsetZ)
+RobotRotationW * theNaoStructure->uLink[NaoStructure::headEndLink].p ;
//now camera pos relative to support foot, translate to pos relative to robot coordinate
Vector3<double> offset = (theNaoStructure->uLink[NaoStructure::bodyLink].p -theNaoStructure->uLink[supfootLink].p);
offset.z = 0;
theCameraMatrix->translation -= offset;
theCameraMatrix->translation.x=theCameraMatrix->translation.x*1000;
theCameraMatrix->translation.y=theCameraMatrix->translation.y*1000;
theCameraMatrix->translation.z=theCameraMatrix->translation.z*1000;
theCameraMatrix->rotation = RHeadW.rotateY(CameraTopRotY);
theCameraMatrix->isValid = true;
}
if(!theCameraConfig->usingTopCam)
{
int supfootLink = 0;
RotationMatrix RHeadW;
RotationMatrix RobotRotationW;
RobotRotationW.rotateX(theNaoStructure->supportFootRotWorld.x).rotateY(theNaoStructure->supportFootRotWorld.y).rotateZ(theNaoStructure->supportFootRotWorld.z);
if(theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_LEFT || theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
supfootLink = NaoStructure::lFootLink;
}else
{
supfootLink = NaoStructure::rFootLink;
}
RHeadW = /*RobotRotationW **/ (theNaoStructure->uLink[NaoStructure::headEndLink].R * theNaoStructure->uLink[supfootLink].R.invert());
theCameraMatrix->translation = RHeadW * Vector3<double>(CameraBottomOffsetX, CameraBottomOffsetY, CameraBottomOffsetZ)
+ RobotRotationW * theNaoStructure->uLink[NaoStructure::headEndLink].p ;
//now camera pos relative to support foot, translate to pos relative to robot coordinate
Vector3<double> offset = (theNaoStructure->uLink[NaoStructure::bodyLink].p -theNaoStructure->uLink[supfootLink].p);
offset.z = 0;
theCameraMatrix->translation -= offset;
theCameraMatrix->translation.x=theCameraMatrix->translation.x*1000;
theCameraMatrix->translation.y=theCameraMatrix->translation.y*1000;
theCameraMatrix->translation.z=theCameraMatrix->translation.z*1000;//视觉的单位为mm
theCameraMatrix->rotation = RHeadW.rotateY(CameraBottomRotY);
theCameraMatrix->isValid = true;
}
}
