nao.cpp

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

/*
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "nao.h"

using namespace std;
using namespace salt;
using namespace boost;

Nao::Nao()
{
    //SetName("nao");

    /** these two values must be in constructor */
    JID_MIN             = JID_ROOT;
    JID_MAX             = JID_RLEG_6;
    PART_MIN            = JID_ROOT;
    PART_MAX            = JID_RLEG_6;

    FootLength          = 0.08;
    FootWidth           = 0.16;
    FootHeight          = 0.03;
    FootMass            = 0.2;

    ShankLength         = 0.08;
    ShankWidth          = 0.07;
    ShankHeight         = 0.11;
    ShankMass           = 0.225;

    ThighLength         = 0.07;
    ThighWidth          = 0.07;
    ThighHeight         = 0.14;
    ThighMass           = 0.275;

    TorsoLength         = 0.1;
    TorsoWidth          = 0.1;
    TorsoHeight         = 0.18;
    TorsoMass           = 1.2171;

    TorsoInitX          = 0.0f;
    TorsoInitY          = 0.0f;
    TorsoInitZ          = 0.385;

    Hip1RelTorso_X      = 0.055;
    Hip1RelTorso_Y      = -0.01;
    Hip1RelTorso_Z      = -0.115;

    Hip2RelHip1_X       = 0;
    Hip2RelHip1_Y       = 0;
    Hip2RelHip1_Z       = 0;

    ThighRelHip2_X      = 0;
    ThighRelHip2_Y      = 0.01;  //set 0.005 to check the tri-angle algorithm
    ThighRelHip2_Z      = -0.04;

    ShankRelThigh_X     = 0;
    ShankRelThigh_Y     = 0.005;
    ShankRelThigh_Z     = -0.125;

    AnkleRelShank_X     = 0;
    AnkleRelShank_Y     = -0.01;
    AnkleRelShank_Z     = -0.055;

    FootRelAnkle_X      = 0;
    FootRelAnkle_Y      = 0.03;
    FootRelAnkle_Z      = -0.035;

    Hip1Axis_X          = -0.7071;
    Hip1Axis_Y          = 0;
    Hip1Axis_Z          = 0.7071;
}

Nao::~Nao()
{
}

void Nao::SetupLinks()
{
    uLINK[JID_ROOT].name         = "j0";
    uLINK[JID_ROOT].eff_name     = "e0";
    uLINK[JID_ROOT].sister       = 0;
    uLINK[JID_ROOT].child        = JID_LLEG_1;
    uLINK[JID_ROOT].mother       = 0;
    uLINK[JID_ROOT].a            = Vector3f(0, 0, 0);
    uLINK[JID_ROOT].c            = Vector3f(0, 0, 0);
    uLINK[JID_ROOT].b            = Vector3f(0, 0, 0);
    uLINK[JID_ROOT].q            = 0;
    uLINK[JID_ROOT].p            = Vector3f(TorsoInitX, TorsoInitY, TorsoInitZ);
    uLINK[JID_ROOT].R.Identity();

    /** ----------------------------- legs begin ---------------------- */
    uLINK[JID_LLEG_1].name       = "llj1";
    uLINK[JID_LLEG_1].eff_name   = "lle1";
    uLINK[JID_LLEG_1].sister     = JID_RLEG_1;
    uLINK[JID_LLEG_1].child      = JID_LLEG_2;
    uLINK[JID_LLEG_1].mother     = JID_ROOT;
    uLINK[JID_LLEG_1].a          = Vector3f(Hip1Axis_X, Hip1Axis_Y, -Hip1Axis_Z);
    uLINK[JID_LLEG_1].c          = Vector3f(0, 0, 0);
    uLINK[JID_LLEG_1].b          = Vector3f(-Hip1RelTorso_X, Hip1RelTorso_Y, Hip1RelTorso_Z)
                                   - uLINK[JID_LLEG_1].c + uLINK[JID_ROOT].c;
    uLINK[JID_LLEG_1].q          = 0;

    uLINK[JID_LLEG_2].name       = "llj2";
    uLINK[JID_LLEG_2].eff_name   = "lle2";
    uLINK[JID_LLEG_2].sister     = 0;
    uLINK[JID_LLEG_2].child      = JID_LLEG_3;
    uLINK[JID_LLEG_2].mother     = JID_LLEG_1;
    uLINK[JID_LLEG_2].a          = Vector3f(0, 1, 0); // Vector3f(1, 0, 0)
    uLINK[JID_LLEG_2].c          = Vector3f(0, 0, 0);
    uLINK[JID_LLEG_2].b          = Vector3f(Hip2RelHip1_X, Hip2RelHip1_Y, Hip2RelHip1_Z)
                                   - uLINK[JID_LLEG_2].c + uLINK[JID_LLEG_1].c;
    uLINK[JID_LLEG_2].q          = 0;

    uLINK[JID_LLEG_3].name       = "llj3";
    uLINK[JID_LLEG_3].eff_name   = "lle3";
    uLINK[JID_LLEG_3].sister     = 0;
    uLINK[JID_LLEG_3].child      = JID_LLEG_4;
    uLINK[JID_LLEG_3].mother     = JID_LLEG_2;
    uLINK[JID_LLEG_3].a          = Vector3f(1, 0, 0); // Vector3f(0, 1, 0)
    uLINK[JID_LLEG_3].c          = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z);
    uLINK[JID_LLEG_3].b          = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z)
                                   - uLINK[JID_LLEG_3].c + uLINK[JID_LLEG_2].c;
    uLINK[JID_LLEG_3].q          = 0;

    uLINK[JID_LLEG_4].name       = "llj4";
    uLINK[JID_LLEG_4].eff_name   = "lle4";
    uLINK[JID_LLEG_4].sister     = 0;
    uLINK[JID_LLEG_4].child      = JID_LLEG_5;
    uLINK[JID_LLEG_4].mother     = JID_LLEG_3;
    uLINK[JID_LLEG_4].a          = Vector3f(1, 0, 0);
    uLINK[JID_LLEG_4].c          = Vector3f(0, 0.01, -0.045);
    uLINK[JID_LLEG_4].b          = Vector3f(ShankRelThigh_X, ShankRelThigh_Y, ShankRelThigh_Z)
                                   - uLINK[JID_LLEG_4].c + uLINK[JID_LLEG_3].c;
    uLINK[JID_LLEG_4].q          = 0;

    uLINK[JID_LLEG_5].name       = "llj5";
    uLINK[JID_LLEG_5].eff_name   = "lle5";
    uLINK[JID_LLEG_5].sister     = 0;
    uLINK[JID_LLEG_5].child      = JID_LLEG_6;
    uLINK[JID_LLEG_5].mother     = JID_LLEG_4;
    uLINK[JID_LLEG_5].a          = Vector3f(1, 0, 0);
    uLINK[JID_LLEG_5].c          = Vector3f(0, 0, 0);
    uLINK[JID_LLEG_5].b          = Vector3f(AnkleRelShank_X, AnkleRelShank_Y, AnkleRelShank_Z)
                                   - uLINK[JID_LLEG_5].c + uLINK[JID_LLEG_4].c;
    uLINK[JID_LLEG_5].q          = 0;

    uLINK[JID_LLEG_6].name       = "llj6";
    uLINK[JID_LLEG_6].eff_name   = "lle6";
    uLINK[JID_LLEG_6].sister     = 0;
    uLINK[JID_LLEG_6].child      = 0;
    uLINK[JID_LLEG_6].mother     = JID_LLEG_5;
    uLINK[JID_LLEG_6].a          = Vector3f(0, 1, 0);
    uLINK[JID_LLEG_6].c          = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z);
    uLINK[JID_LLEG_6].b          = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z)
                                   - uLINK[JID_LLEG_6].c + uLINK[JID_LLEG_5].c;
    uLINK[JID_LLEG_6].q          = 0;

    uLINK[JID_RLEG_1].name       = "rlj1";
    uLINK[JID_RLEG_1].eff_name   = "rle1";
    uLINK[JID_RLEG_1].sister     = 0;
    uLINK[JID_RLEG_1].child      = JID_RLEG_2;
    uLINK[JID_RLEG_1].mother     = JID_ROOT;
    uLINK[JID_RLEG_1].a          = Vector3f(Hip1Axis_X, Hip1Axis_Y, Hip1Axis_Z);
    uLINK[JID_RLEG_1].c          = Vector3f(0, 0, 0);
    uLINK[JID_RLEG_1].b          = Vector3f(Hip1RelTorso_X, Hip1RelTorso_Y, Hip1RelTorso_Z)
                                   - uLINK[JID_RLEG_1].c + uLINK[JID_ROOT].c;
    uLINK[JID_RLEG_1].q          = 0;

    uLINK[JID_RLEG_2].name       = "rlj2";
    uLINK[JID_RLEG_2].eff_name   = "rle2";
    uLINK[JID_RLEG_2].sister     = 0;
    uLINK[JID_RLEG_2].child      = JID_RLEG_3;
    uLINK[JID_RLEG_2].mother     = JID_RLEG_1;
    uLINK[JID_RLEG_2].a          = Vector3f(0, 1, 0); // Vector3f(1, 0, 0)
    uLINK[JID_RLEG_2].c          = Vector3f(0, 0, 0);
    uLINK[JID_RLEG_2].b          = Vector3f(Hip2RelHip1_X, Hip2RelHip1_Y, Hip2RelHip1_Z)
                                   - uLINK[JID_RLEG_2].c + uLINK[JID_RLEG_1].c;
    uLINK[JID_RLEG_2].q          = 0;

    uLINK[JID_RLEG_3].name      = "rlj3";
    uLINK[JID_RLEG_3].eff_name  = "rle3";
    uLINK[JID_RLEG_3].sister    = 0;
    uLINK[JID_RLEG_3].child     = JID_RLEG_4;
    uLINK[JID_RLEG_3].mother    = JID_RLEG_2;
    uLINK[JID_RLEG_3].a         = Vector3f(1, 0, 0); // Vector3f(0, 1, 0)
    uLINK[JID_RLEG_3].c         = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z);
    uLINK[JID_RLEG_3].b         = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z)
                                  - uLINK[JID_RLEG_3].c + uLINK[JID_RLEG_2].c;
    uLINK[JID_RLEG_3].q         = 0;

    uLINK[JID_RLEG_4].name      = "rlj4";
    uLINK[JID_RLEG_4].eff_name  = "rle4";
    uLINK[JID_RLEG_4].sister    = 0;
    uLINK[JID_RLEG_4].child     = JID_RLEG_5;
    uLINK[JID_RLEG_4].mother    = JID_RLEG_3;
    uLINK[JID_RLEG_4].a         = Vector3f(1, 0, 0);
    uLINK[JID_RLEG_4].c         = Vector3f(0, 0.01, -0.045);
    uLINK[JID_RLEG_4].b         = Vector3f(ShankRelThigh_X, ShankRelThigh_Y, ShankRelThigh_Z)
                                  - uLINK[JID_RLEG_4].c + uLINK[JID_RLEG_3].c;
    uLINK[JID_RLEG_4].q         = 0;

    uLINK[JID_RLEG_5].name      = "rlj5";
    uLINK[JID_RLEG_5].eff_name  = "rle5";
    uLINK[JID_RLEG_5].sister    = 0;
    uLINK[JID_RLEG_5].child     = JID_RLEG_6;
    uLINK[JID_RLEG_5].mother    = JID_RLEG_4;
    uLINK[JID_RLEG_5].a         = Vector3f(1, 0, 0);
    uLINK[JID_RLEG_5].c         = Vector3f(0, 0, 0);
    uLINK[JID_RLEG_5].b         = Vector3f(AnkleRelShank_X, AnkleRelShank_Y, AnkleRelShank_Z)
                                  - uLINK[JID_RLEG_5].c + uLINK[JID_RLEG_4].c;
    uLINK[JID_RLEG_5].q         = 0;

    uLINK[JID_RLEG_6].name      = "rlj6";
    uLINK[JID_RLEG_6].eff_name  = "rle6";
    uLINK[JID_RLEG_6].sister    = 0;
    uLINK[JID_RLEG_6].child     = 0;
    uLINK[JID_RLEG_6].mother    = JID_RLEG_5;
    uLINK[JID_RLEG_6].a         = Vector3f(0, 1, 0);
    uLINK[JID_RLEG_6].c         = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z);
    uLINK[JID_RLEG_6].b         = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z)
                                  - uLINK[JID_RLEG_6].c + uLINK[JID_RLEG_5].c;
    uLINK[JID_RLEG_6].q         = 0;
    /** ------------------------------ legs end ----------------------- */

    /** head */
    uLINK[JID_HEAD_2].name      = "hj2";
    uLINK[JID_HEAD_2].eff_name  = "he2";
    uLINK[JID_HEAD_1].name      = "hj1";
    uLINK[JID_HEAD_1].eff_name  = "he1";

    /** ----------------------------- arms begin ---------------------- */
    uLINK[JID_LARM_1].name      = "laj1";
    uLINK[JID_LARM_1].eff_name  = "lae1";
    uLINK[JID_LARM_2].name      = "laj2";
    uLINK[JID_LARM_2].eff_name  = "lae2";
    uLINK[JID_LARM_3].name      = "laj3";
    uLINK[JID_LARM_3].eff_name  = "lae3";
    uLINK[JID_LARM_4].name      = "laj4";
    uLINK[JID_LARM_4].eff_name  = "lae4";

    uLINK[JID_RARM_1].name      = "raj1";
    uLINK[JID_RARM_1].eff_name  = "rae1";
    uLINK[JID_RARM_2].name      = "raj2";
    uLINK[JID_RARM_2].eff_name  = "rae2";