nao.cpp

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

    uLINK[JID_RARM_3].name      = "raj3";
    uLINK[JID_RARM_3].eff_name  = "rae3";
    uLINK[JID_RARM_4].name      = "raj4";
    uLINK[JID_RARM_4].eff_name  = "rae4";
    /** ------------------------------ arms end ----------------------- */

    ForwardKinematics(JID_ROOT);

    NAO_A               = (uLINK[JID_LLEG_4].p - uLINK[JID_LLEG_3].p).Length();
    NAO_B               = (uLINK[JID_LLEG_5].p - uLINK[JID_LLEG_4].p).Length();   
    NAO_Dl              = uLINK[JID_LLEG_3].p - uLINK[JID_ROOT].p;
    NAO_Dr              = uLINK[JID_RLEG_3].p - uLINK[JID_ROOT].p;
    NAO_E               = - uLINK[JID_LLEG_6].c;

    /** cut a branch(sister) can make IK faster */
    // uLINK[JID_LLEG_1].sister = 0;
}

void Nao::SetupRobotPartInfo()
{
    // NOTE to be completed
    mRobotPartInfo[PART_BODY].name  = "torso";
}

bool Nao::IK_leg(const Matrix& hip, float A, float B,
                 const Matrix& ankle, bool left)
{
    Matrix invHip = hip;
    invHip.InvertRotationMatrix();
    Matrix invAnkle = ankle;
    invAnkle.InvertRotationMatrix();

    Vector3f r = invAnkle.Rotate(hip.Pos() - ankle.Pos());
    float C = r.Length();
    float cosKneePitch = (C * C - A * A - B * B) / (2.0f * A * B);
    float kneePitch = 0.0f;

    if (cosKneePitch >= 1.0f)
    {
        kneePitch = 0.0f;
        cerr << "cosKneePitch >= 1.0" << endl;
        return false;
    }
    else if (cosKneePitch <= -1.0f)
    {
        kneePitch = gPI;
        cerr << "cosKneePitch <= -1.0" << endl;
        return false;
    }
    else
        kneePitch = gArcCos(cosKneePitch);

    kneePitch *= -1.0;

    float q6a = gArcSin((A / C) * gSin(gPI - kneePitch));
    float ankleRoll = -gArcTan2(r.x(), r.z());

    float anklePitch = gArcTan2(
                        r.y(),
                        gSign(r.z()) * gSqrt(r.x() * r.x() + r.z() * r.z())
                       ) - q6a;

    Matrix R = invHip * ankle;

    R.RotateY(-ankleRoll);
    R.RotateX(-anklePitch);
    R.RotateX(-kneePitch);

    float hipPitch      = 0.0f;
    float hipRoll       = 0.0f;
    float hipYaw        = 0.0f;
    float cosHipYaw     = 0.0f;
    float sinHipYaw     = 0.0f;
    float cosHipRoll    = 0.0f;
    float sinHipRoll    = 0.0f;

    if (left)
    {
        hipPitch = gArcTan2(R(2,1) + R(0,1), R(2,2) + R(0,2));
        hipPitch = hipPitch >  gHalfPI ? hipPitch - gPI :
                   hipPitch < -gHalfPI ? hipPitch + gPI : hipPitch;

        float cosHipPitch   = gCos(hipPitch);
        float addCSHipRoll  = (R(2,2) + R(0,2)) / cosHipPitch;

        cosHipYaw = (R(2,0) - R(0,0)) / addCSHipRoll;
        sinHipYaw = R(1,0) / (-0.7071 * addCSHipRoll);
        hipYaw = gArcTan2(sinHipYaw, cosHipYaw);
        hipYaw = -hipYaw; // special in left leg
        hipYaw = hipYaw >  gHalfPI ? hipYaw - gPI :
                 hipYaw < -gHalfPI ? hipYaw + gPI : hipYaw;

        cosHipYaw = cos(hipYaw);

        float subCSHipRoll = 2 * R(0,0) - cosHipYaw * addCSHipRoll;
        cosHipRoll = addCSHipRoll + subCSHipRoll;
        sinHipRoll = addCSHipRoll - subCSHipRoll;
        hipRoll = gArcTan2(sinHipRoll, cosHipRoll);
        hipRoll = hipRoll >  gHalfPI ? hipRoll - gPI :
                  hipRoll < -gHalfPI ? hipRoll + gPI : hipRoll;
    }
    else
    {
        hipPitch = gArcTan2(R(2,1) + R(0,1), R(2,2) + R(0,2));
        hipPitch = hipPitch >  gHalfPI ? hipPitch - gPI :
                   hipPitch < -gHalfPI ? hipPitch + gPI : hipPitch;

        float cosHipPitch   = gCos(hipPitch);
        float subCSHipRoll  = (R(2,2) - R(0,2)) / cosHipPitch;

        cosHipYaw = (R(2,0) + R(0,0)) / subCSHipRoll;
        sinHipYaw = R(1,0) / (0.7071 * subCSHipRoll);
        hipYaw = gArcTan2(sinHipYaw, cosHipYaw);
        hipYaw = hipYaw >  gHalfPI ? hipYaw - gPI :
                 hipYaw < -gHalfPI ? hipYaw + gPI : hipYaw;
        cosHipYaw = cos(hipYaw);

        float addCSHipRoll = 2 * R(0,0) - cosHipYaw * subCSHipRoll;
        cosHipRoll = addCSHipRoll + subCSHipRoll;
        sinHipRoll = addCSHipRoll - subCSHipRoll;
        hipRoll = gArcTan2(sinHipRoll, cosHipRoll);
        hipRoll = hipRoll >  gHalfPI ? hipRoll - gPI :
                  hipRoll < -gHalfPI ? hipRoll + gPI : hipRoll;
    }

    if (left)
    {
        mIKJointAngle[Nao::JID_LLEG_1]  = hipYaw;
        mIKJointAngle[Nao::JID_LLEG_2]  = hipRoll;
        mIKJointAngle[Nao::JID_LLEG_3]  = hipPitch;
        mIKJointAngle[Nao::JID_LLEG_4]  = kneePitch;
        mIKJointAngle[Nao::JID_LLEG_5]  = anklePitch;
        mIKJointAngle[Nao::JID_LLEG_6]  = ankleRoll;
    }
    else
    {
        mIKJointAngle[Nao::JID_RLEG_1] = hipYaw;
        mIKJointAngle[Nao::JID_RLEG_2] = hipRoll;
        mIKJointAngle[Nao::JID_RLEG_3] = hipPitch;
        mIKJointAngle[Nao::JID_RLEG_4] = kneePitch;
        mIKJointAngle[Nao::JID_RLEG_5] = anklePitch;
        mIKJointAngle[Nao::JID_RLEG_6] = ankleRoll;
    }

    return true;
}

bool Nao::IK_leg(const salt::Matrix& torso, const salt::Matrix& foot, bool left)
{
    uLINK[JID_ROOT].R = torso;
    uLINK[JID_ROOT].p = torso.Pos();

    Vector3f D      = left ? NAO_Dl : NAO_Dr;
    JointID to ;     //= left ? JID_LLEG_6 : JID_RLEG_6;
	if(left) to = JID_LLEG_6 ;
	else to = JID_RLEG_6;
    Matrix hip      = torso;
    hip.Pos()       = torso * D;
    Matrix ankle    = foot;
    ankle.Pos()     = foot * NAO_E;

    if (! IK_leg(hip, NAO_A, NAO_B, ankle, left))
    {
        cerr << "analytic method failed: leg -- "
                          << left << endl;
        return false;
    }

    if (left)
    {
        uLINK[Nao::JID_LLEG_1].q = mIKJointAngle[Nao::JID_LLEG_1];
        uLINK[Nao::JID_LLEG_2].q = mIKJointAngle[Nao::JID_LLEG_2];
        uLINK[Nao::JID_LLEG_3].q = mIKJointAngle[Nao::JID_LLEG_3];
        uLINK[Nao::JID_LLEG_4].q = mIKJointAngle[Nao::JID_LLEG_4];
        uLINK[Nao::JID_LLEG_5].q = mIKJointAngle[Nao::JID_LLEG_5];
        uLINK[Nao::JID_LLEG_6].q = mIKJointAngle[Nao::JID_LLEG_6];
    }
    else
    {
        uLINK[Nao::JID_RLEG_1].q = mIKJointAngle[Nao::JID_RLEG_1];
        uLINK[Nao::JID_RLEG_2].q = mIKJointAngle[Nao::JID_RLEG_2];
        uLINK[Nao::JID_RLEG_3].q = mIKJointAngle[Nao::JID_RLEG_3];
        uLINK[Nao::JID_RLEG_4].q = mIKJointAngle[Nao::JID_RLEG_4];
        uLINK[Nao::JID_RLEG_5].q = mIKJointAngle[Nao::JID_RLEG_5];
        uLINK[Nao::JID_RLEG_6].q = mIKJointAngle[Nao::JID_RLEG_6];
    }

    ForwardKinematics(JID_ROOT); // apply the joint angles before InverseKinematics
    if (! InverseKinematics(to, ankle))
    {
        cerr << "numerical method failed: leg -- "
                          << left << endl;
        return false;
    }

    if (left)
    {
        mIKJointAngle[Nao::JID_LLEG_1] = uLINK[Nao::JID_LLEG_1].q;
        mIKJointAngle[Nao::JID_LLEG_2] = uLINK[Nao::JID_LLEG_2].q;
        mIKJointAngle[Nao::JID_LLEG_3] = uLINK[Nao::JID_LLEG_3].q;
        mIKJointAngle[Nao::JID_LLEG_4] = uLINK[Nao::JID_LLEG_4].q;
        mIKJointAngle[Nao::JID_LLEG_5] = uLINK[Nao::JID_LLEG_5].q;
        mIKJointAngle[Nao::JID_LLEG_6] = uLINK[Nao::JID_LLEG_6].q;
    }
    else
    {
        mIKJointAngle[Nao::JID_RLEG_1] = uLINK[Nao::JID_RLEG_1].q;
        mIKJointAngle[Nao::JID_RLEG_2] = uLINK[Nao::JID_RLEG_2].q;
        mIKJointAngle[Nao::JID_RLEG_3] = uLINK[Nao::JID_RLEG_3].q;
        mIKJointAngle[Nao::JID_RLEG_4] = uLINK[Nao::JID_RLEG_4].q;
        mIKJointAngle[Nao::JID_RLEG_5] = uLINK[Nao::JID_RLEG_5].q;
        mIKJointAngle[Nao::JID_RLEG_6] = uLINK[Nao::JID_RLEG_6].q;
    }
    return true;
}

void Nao::CalcLegJointVel(const salt::Vector3f& vb, const salt::Vector3f& wb,
                          const salt::Vector3f& vt, const salt::Vector3f& wt,
                          bool left)
{
    int to     ;// = left ? JID_LLEG_6 : JID_RLEG_6;
	if(left) to = JID_LLEG_6 ;
	else to = JID_RLEG_6;
    float* vel  = mIKJointAngle.get();
    TIndex idx  = FindRoute(to);
    int jsize   = idx.size();

    Vector3f vd, vw;
    vd = vt - vb - wb.Cross(uLINK[to].p - uLINK[1].p);
    vw = wt - wb;

    float v[6];
    v[0] = vd.x(); v[1] = vd.y(); v[2] = vd.z();
    v[3] = vw.x(); v[4] = vw.y(); v[5] = vw.z();

    // allot memory for Jacobian matrix
    float** J = new float*[6];
    for (int i = 0; i < 6; ++i)
    {
        J[i] = new float[jsize];
    }

    CalcJacobian(J, idx);
    Solve(vel, (const float **)J, (const float *)v, 6, jsize);

    for (int i = 0; i < 6; ++i)
    {
        delete [] J[i];
        J[i] = NULL;
    }
    delete [] J;
    J = NULL;
}