soccerbot.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 "soccerbot.h"
//#include <zeitgeist/logserver/logserver.h>

using namespace std;
using namespace salt;
using namespace boost;
//using namespace zeitgeist;

Soccerbot::Soccerbot()
{
    //SetName("soccerbot");

    /** 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;

    Scale               = 0.1;
    FootLength          = 0.6 * Scale;
    FootWidth           = 0.956 * Scale;
    FootHeight          = 0.095 * Scale;
    FootMass            = 0.1;

    ShankLength         = 0.52 * Scale;
    ShankWidth          = 0.45 * Scale;
    ShankHeight         = 0.964 * Scale;
    ShankMass           = 0.25;

    ThighLength         = 0.56 * Scale;
    ThighWidth          = 0.45 * Scale;
    ThighHeight         = 1.3 * Scale;
    ThighMass           = 0.25;

    HipLength           = 0.273 * Scale;
    HipWidth            = 0.273 * Scale;
    HipHeight           = 0.3 * Scale;
    HiphMass            = 0.1;

    LowerTorsoLength    = 1.32 * Scale;
    LowerTorsoWidth     = 0.55 * Scale;
    LowerTorsoHeight    = 0.55 * Scale;
    LowerTorsoMass      = 0.6;

    UpperTorsoLength    = 1.37 * Scale;
    UpperTorsoWidth     = 0.96 * Scale;
    UpperTorsoHeight    = 1 * Scale;
    UpperTorsoMass      = 1.2;

    UpperTorsoPosX     = 5;
    UpperTorsoPosY     = 5;
    UpperTorsoPosZ     = 3.75;

    LowerRelUpperTorso_X    = 0;
    LowerRelUpperTorso_Y    = 0;
    LowerRelUpperTorso_Z    = - UpperTorsoHeight/2 - LowerTorsoHeight/2 + 0.25*Scale;

    HipRelUpperTorso_X      = LowerRelUpperTorso_X - LowerTorsoLength/4 - 0.06*Scale;
    HipRelUpperTorso_Y      = LowerRelUpperTorso_Y;
    HipRelUpperTorso_Z      = LowerRelUpperTorso_Z - LowerTorsoHeight/2 - HipHeight/2;

    ThighRelHip_X           = 0;
    ThighRelHip_Y           = 0;
    ThighRelHip_Z           = - HipHeight/2 - ThighHeight/2;

    ShankRelThigh_X         = 0;
    ShankRelThigh_Y         = 0;
    ShankRelThigh_Z         = - ThighHeight/2.0 - ShankHeight/2.0 - 0.05*Scale;

    FootRelShank_X          = 0;
    FootRelShank_Y          = 0;
    FootRelShank_Z          = - ShankHeight/2.0 - FootHeight/2.0 - 0.05*Scale;
}

Soccerbot::~Soccerbot()
{
}

void Soccerbot::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(UpperTorsoPosX, UpperTorsoPosY, UpperTorsoPosZ);
    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(0, 0, 1);
    uLINK[JID_LLEG_1].c          = Vector3f(0, 0, -HipHeight*0.5);
    uLINK[JID_LLEG_1].b          = Vector3f(HipRelUpperTorso_X,
                                            HipRelUpperTorso_Y,
                                            HipRelUpperTorso_Z)
                                   - uLINK[JID_LLEG_1].c + uLINK[JID_ROOT].c;
    uLINK[JID_LLEG_1].q          = 0;

    uLINK[JID_LLEG_2].name       = "llj2_3";
    uLINK[JID_LLEG_2].eff_name   = "lle2_3";
    uLINK[JID_LLEG_2].twin       = JID_LLEG_3;
    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(1, 0, 0);
    uLINK[JID_LLEG_2].c          = Vector3f(0, 0, -ThighHeight/2.0);
    uLINK[JID_LLEG_2].b          = Vector3f(ThighRelHip_X, ThighRelHip_Y, ThighRelHip_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   = "lle2_3";
    uLINK[JID_LLEG_3].twin       = JID_LLEG_2;
    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(0, 1, 0);
    uLINK[JID_LLEG_3].c          = Vector3f(0, 0, -ThighHeight/2.0);
    uLINK[JID_LLEG_3].b          = Vector3f(0, 0, 0);
    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, -ShankHeight*0.5);
    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_6";
    uLINK[JID_LLEG_5].eff_name   = "lle5_6";
    uLINK[JID_LLEG_5].twin       = JID_LLEG_6;
    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, -FootHeight*3.0);
    uLINK[JID_LLEG_5].b          = Vector3f(FootRelShank_X, FootRelShank_Y, FootRelShank_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   = "lle5_6";
    uLINK[JID_LLEG_6].twin       = JID_LLEG_5;
    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(0, 0, -FootHeight*3.0);
    uLINK[JID_LLEG_6].b          = Vector3f(0, 0, 0);
    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(0, 0, 1);
    uLINK[JID_RLEG_1].c          = Vector3f(0, 0, -HipHeight*0.5);
    uLINK[JID_RLEG_1].b          = Vector3f(-HipRelUpperTorso_X,
                                             HipRelUpperTorso_Y,
                                             HipRelUpperTorso_Z)
                                   - uLINK[JID_LLEG_1].c + uLINK[JID_ROOT].c;
    uLINK[JID_RLEG_1].q          = 0;

    uLINK[JID_RLEG_2].name       = "rlj2_3";
    uLINK[JID_RLEG_2].eff_name   = "rle2_3";
    uLINK[JID_RLEG_2].twin       = JID_RLEG_3;
    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(1, 0, 0);
    uLINK[JID_RLEG_2].c          = Vector3f(0, 0, -ThighHeight/2.0);
    uLINK[JID_RLEG_2].b          = Vector3f(ThighRelHip_X, ThighRelHip_Y, ThighRelHip_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   = "rle2_3";
    uLINK[JID_RLEG_3].twin       = JID_RLEG_2;
    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(0, 1, 0);
    uLINK[JID_RLEG_3].c          = Vector3f(0, 0, -ThighHeight/2.0);
    uLINK[JID_RLEG_3].b          = Vector3f(0, 0, 0);
    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, -ShankHeight*0.5);
    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_6";
    uLINK[JID_RLEG_5].eff_name   = "rle5_6";
    uLINK[JID_RLEG_5].twin       = JID_RLEG_6;
    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, -FootHeight*3.0);
    uLINK[JID_RLEG_5].b          = Vector3f(FootRelShank_X, FootRelShank_Y, FootRelShank_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   = "rle5_6";
    uLINK[JID_RLEG_6].twin       = JID_RLEG_5;
    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(0, 0, -FootHeight*3.0);
    uLINK[JID_RLEG_6].b          = Vector3f(0, 0, 0);
    uLINK[JID_RLEG_6].q          = 0;
    /** ------------------------------ legs end ----------------------- */