soccerbot.cpp

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

    /** head */
    uLINK[JID_HEAD_1].name       = "hj";
    uLINK[JID_HEAD_1].eff_name   = "he";
    uLINK[JID_HEAD_1].twin       = JID_HEAD_2;
    uLINK[JID_HEAD_1].child      = JID_HEAD_2;
    uLINK[JID_HEAD_2].name       = "hj2";
    uLINK[JID_HEAD_2].eff_name   = "he";
    uLINK[JID_HEAD_2].twin       = JID_HEAD_1;
    uLINK[JID_HEAD_2].child      = 0;


    /** ----------------------------- arms begin ---------------------- */
    uLINK[JID_LARM_1].name       = "laj1_2";
    uLINK[JID_LARM_1].eff_name   = "lae1_2";
    uLINK[JID_LARM_1].twin       = JID_LARM_2;
    uLINK[JID_LARM_1].child      = JID_LARM_2;
    uLINK[JID_LARM_2].name       = "laj2";
    uLINK[JID_LARM_2].eff_name   = "lae1_2";
    uLINK[JID_LARM_2].twin       = JID_LARM_1;
    uLINK[JID_LARM_2].child      = JID_LARM_3;
    uLINK[JID_LARM_3].name       = "laj3";
    uLINK[JID_LARM_3].eff_name   = "lae3";
    uLINK[JID_LARM_3].child      = JID_LARM_4;
    uLINK[JID_LARM_4].name       = "laj4";
    uLINK[JID_LARM_4].eff_name   = "lae4";
    uLINK[JID_LARM_4].child      = 0;

    uLINK[JID_RARM_1].name       = "raj1_2";
    uLINK[JID_RARM_1].eff_name   = "rae1_2";
    uLINK[JID_RARM_1].twin       = JID_RARM_2;
    uLINK[JID_RARM_1].child      = JID_RARM_2;
    uLINK[JID_RARM_2].name       = "raj2";
    uLINK[JID_RARM_2].eff_name   = "rae1_2";
    uLINK[JID_RARM_2].twin       = JID_RARM_1;
    uLINK[JID_RARM_2].child      = JID_RARM_3;
    uLINK[JID_RARM_3].name       = "raj3";
    uLINK[JID_RARM_3].eff_name   = "rae3";
    uLINK[JID_RARM_3].child      = JID_RARM_4;
    uLINK[JID_RARM_4].name       = "raj4";
    uLINK[JID_RARM_4].eff_name   = "rae4";
    uLINK[JID_RARM_4].child      = 0;
    /** ------------------------------ arms end ----------------------- */

    ForwardKinematics(JID_ROOT);

    SOCCERBOT_A         = (uLINK[JID_LLEG_4].p - uLINK[JID_LLEG_3].p).Length();
    SOCCERBOT_B         = (uLINK[JID_LLEG_5].p - uLINK[JID_LLEG_4].p).Length();   
    SOCCERBOT_Dl        = uLINK[JID_LLEG_3].p - uLINK[JID_ROOT].p;
    SOCCERBOT_Dr        = uLINK[JID_RLEG_3].p - uLINK[JID_ROOT].p;
    SOCCERBOT_E         = - uLINK[JID_LLEG_6].c;

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

void Soccerbot::SetupRobotPartInfo()
{
    // NOTE to be done
}

bool Soccerbot::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 hipYaw        = 0.0f;
    float hipPitch      = 0.0f;
    float hipRoll       = 0.0f;

    hipYaw = gArcTan2(-R(0,1), R(1,1));
    hipRoll = gArcTan2(-R(2,0), R(2,2));
    float cz = gCos(hipYaw);
    float sz = gSin(hipYaw);
    hipPitch = gArcTan2(R(2,1), -R(0,1) * sz + R(1,1) * cz);

    // float cy = gCos(hipRoll);
    // float sy = gSin(hipRoll);
    // hipPitch = gArcTan2(R(2,1), -R(2,0) * sy + R(2,2) * cy);

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

    return true;
}

bool Soccerbot::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 ? SOCCERBOT_Dl : SOCCERBOT_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 * SOCCERBOT_E;

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

    if (left)
    {
        uLINK[JID_LLEG_1].q = mIKJointAngle[JID_LLEG_1];
        uLINK[JID_LLEG_2].q = mIKJointAngle[JID_LLEG_2];
        uLINK[JID_LLEG_3].q = mIKJointAngle[JID_LLEG_3];
        uLINK[JID_LLEG_4].q = mIKJointAngle[JID_LLEG_4];
        uLINK[JID_LLEG_5].q = mIKJointAngle[JID_LLEG_5];
        uLINK[JID_LLEG_6].q = mIKJointAngle[JID_LLEG_6];
    }
    else
    {
        uLINK[JID_RLEG_1].q = mIKJointAngle[JID_RLEG_1];
        uLINK[JID_RLEG_2].q = mIKJointAngle[JID_RLEG_2];
        uLINK[JID_RLEG_3].q = mIKJointAngle[JID_RLEG_3];
        uLINK[JID_RLEG_4].q = mIKJointAngle[JID_RLEG_4];
        uLINK[JID_RLEG_5].q = mIKJointAngle[JID_RLEG_5];
        uLINK[JID_RLEG_6].q = mIKJointAngle[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[JID_LLEG_1] = uLINK[JID_LLEG_1].q;
        mIKJointAngle[JID_LLEG_2] = uLINK[JID_LLEG_2].q;
        mIKJointAngle[JID_LLEG_3] = uLINK[JID_LLEG_3].q;
        mIKJointAngle[JID_LLEG_4] = uLINK[JID_LLEG_4].q;
        mIKJointAngle[JID_LLEG_5] = uLINK[JID_LLEG_5].q;
        mIKJointAngle[JID_LLEG_6] = uLINK[JID_LLEG_6].q;
    }
    else
    {
        mIKJointAngle[JID_RLEG_1] = uLINK[JID_RLEG_1].q;
        mIKJointAngle[JID_RLEG_2] = uLINK[JID_RLEG_2].q;
        mIKJointAngle[JID_RLEG_3] = uLINK[JID_RLEG_3].q;
        mIKJointAngle[JID_RLEG_4] = uLINK[JID_RLEG_4].q;
        mIKJointAngle[JID_RLEG_5] = uLINK[JID_RLEG_5].q;
        mIKJointAngle[JID_RLEG_6] = uLINK[JID_RLEG_6].q;
    }
    return true;
}

void Soccerbot::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;
}