soccerbot.h

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.
*/
#ifndef SOCCERBOT_H
#define SOCCERBOT_H

#include "robot.h"

class Soccerbot : public Robot
{
public:
    static const JointID JID_HEAD_1 = 2;
    static const JointID JID_HEAD_2 = 3;
    static const JointID JID_LARM_1 = 4;
    static const JointID JID_RARM_1 = 5;
    static const JointID JID_LARM_2 = 6;
    static const JointID JID_RARM_2 = 7;
    static const JointID JID_LARM_3 = 8;
    static const JointID JID_RARM_3 = 9;
    static const JointID JID_LARM_4 = 10;
    static const JointID JID_RARM_4 = 11;
    static const JointID JID_LLEG_1 = 12;
    static const JointID JID_RLEG_1 = 13;
    static const JointID JID_LLEG_2 = 14;
    static const JointID JID_RLEG_2 = 15;
    static const JointID JID_LLEG_3 = 16;
    static const JointID JID_RLEG_3 = 17;
    static const JointID JID_LLEG_4 = 18;
    static const JointID JID_RLEG_4 = 19;
    static const JointID JID_LLEG_5 = 20;
    static const JointID JID_RLEG_5 = 21;
    static const JointID JID_LLEG_6 = 22;
    static const JointID JID_RLEG_6 = 23;

public:
    Soccerbot();
    virtual ~Soccerbot();

    bool IK_leg(const salt::Matrix& torso, const salt::Matrix& foot, bool left);

    void CalcLegJointVel(const salt::Vector3f& vb, const salt::Vector3f& wb,
                         const salt::Vector3f& vt, const salt::Vector3f& wt,
                         bool left);

protected:
    virtual void SetupLinks();

    virtual void SetupRobotPartInfo();

    bool IK_leg(const salt::Matrix& hip, float A, float B,
                const salt::Matrix& ankle, bool left);

private:
    float Scale;

    /** foots */
    float FootLength;
    float FootWidth;
    float FootHeight;
    float FootMass;

    float FootRelShank_X;
    float FootRelShank_Y;
    float FootRelShank_Z;

    /** shanks */
    float ShankLength;
    float ShankWidth;
    float ShankHeight;
    float ShankMass;

    float ShankRelThigh_X;
    float ShankRelThigh_Y;
    float ShankRelThigh_Z;

    /** thighs */
    float ThighLength;
    float ThighWidth;
    float ThighHeight;
    float ThighMass;

    float ThighRelHip_X;
    float ThighRelHip_Y;
    float ThighRelHip_Z;

    /** hips */
    float HipLength;
    float HipWidth;
    float HipHeight;
    float HiphMass;

    float HipRelUpperTorso_X;
    float HipRelUpperTorso_Y;
    float HipRelUpperTorso_Z;

    /** lower torso */
    float LowerTorsoLength;
    float LowerTorsoWidth;
    float LowerTorsoHeight;
    float LowerTorsoMass;

    float LowerRelUpperTorso_X;
    float LowerRelUpperTorso_Y;
    float LowerRelUpperTorso_Z;

    /** upper torso */
    float UpperTorsoLength;
    float UpperTorsoWidth;
    float UpperTorsoHeight;
    float UpperTorsoMass;

    float UpperTorsoPosX;
    float UpperTorsoPosY;
    float UpperTorsoPosZ;

    /** distance and vector */
    float SOCCERBOT_A;
    float SOCCERBOT_B;
    salt::Vector3f SOCCERBOT_Dl;
    salt::Vector3f SOCCERBOT_Dr;
    salt::Vector3f SOCCERBOT_E;
};

#endif