action.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 "action.h"
//#include "log"
#include "./salt/geometry.h"
#include "./RobotModel/robot.h"
#include "./WorldModel/self.h"
#include "./RobotModel/nao.h"

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


Action::Action() :
    mIsDone(false)
{	
}

Action::~Action()
{
}

void Action::Init()
{
    mJointAngle.reset(new float[NAO->GetJointMax() + 1]);
    mJointVel.reset(new float[NAO->GetJointMax() + 1]);
}

void Action::OnUnlink()
{
    mRobot.reset();
    mSelf.reset();
}

bool Action::DoAction()
{	//cout << "Action::DoAction" << endl;
    Update();
    Execute();
    GeneralActionCommand();
    return true;
}

void Action::Update()
{	
    const shared_array<Robot::Link> link = NAO->GetLink();
    if (link.get() != 0)
    {	
        for (int i = NAO->GetJointMin(); i <= NAO->GetJointMax(); ++i)
        {	//cout << "Action::Update----" << "i:" << i << endl;
            if (i == Robot::JID_ROOT) continue;
            /** NOTE if the link is the top one or not been
            * set up correctly, it will not be updated */
            // if (link[i].mother == 0) continue;
            mJointAngle[i] = link[i].q;
        }
    }
	
    for (int i = NAO->GetJointMin(); i <= NAO->GetJointMax(); ++i)
    {
        mJointVel[i] = 0.0f;
    }
}

void Action::GeneralActionCommand()
{
    stringstream ss;

    const shared_array<Robot::Link> link = NAO->GetLink();
    if (link.get() != 0)
    {
        for (int i = NAO->GetJointMin(); i <= NAO->GetJointMax(); ++i)
        {
            if (i == Robot::JID_ROOT) continue;
            /** NOTE if the link is the top one or not been set
             * up correctly, its joint velocity will not be sent */
            // if (link[i].mother == 0) continue;
            int twin = link[i].twin;
            if (twin == 0) // hinge joint effector
            {
                ss << "(" << link[i].eff_name;
                ss << " " << Precision(mJointVel[i]) << ")";
            }
            else // universal joint effector
            {
                int child = link[i].child;
                if (twin == child) // first twin
                {
                    ss << "(" << link[i].eff_name;
                    ss << " " << Precision(mJointVel[i]);
                    ss << " " << Precision(mJointVel[twin]) << ")";
                }
            }
        }
    }
    mActionCommand = ss.str();
    	cout << mActionCommand << endl;
	cout << "****************************************the end of send message****************************************" << endl;
}

void Action::CalculateVel(JointID id, float angle, float maxVel)
{
    if (maxVel < 0)
    {
        cout << "NaoAction ERROR: (CalculateVel) "
                          << "maxVel < 0" << endl;
        mJointVel[id] = 0.0f;
        return ;
    }

    float curAngle = gRadToDeg(mJointAngle[id]);
    float minus = gNormalizeDeg(angle - curAngle);
    float vel = 0.0;

    vel = gAbs(minus) > maxVel ? maxVel * gSign(minus) : minus;
    vel = std::min(gDegToRad(vel) * 10.0f, 100.0f);
	//aLOG << "minus: " << minus << "       vel: " << vel << endl;
    mJointVel[id] = vel;
}