ZeroForceControlVelocityCalcuation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotSkillTemplates::ForceControlGroup
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ZeroForceControlVelocityCalcuation.h"
 
#include <RobotSkillTemplates/statecharts/ForceControlGroup/ForceControlGroupStatechartContext.generated.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <cmath>
 
 
using namespace armarx;
using namespace ForceControlGroup;
 
// DO NOT EDIT NEXT LINE
ZeroForceControlVelocityCalcuation::SubClassRegistry ZeroForceControlVelocityCalcuation::Registry(ZeroForceControlVelocityCalcuation::GetName(), &ZeroForceControlVelocityCalcuation::CreateInstance);
 
 
 
ZeroForceControlVelocityCalcuation::ZeroForceControlVelocityCalcuation(XMLStateConstructorParams stateData) :
    XMLStateTemplate<ZeroForceControlVelocityCalcuation>(stateData),  ZeroForceControlVelocityCalcuationGeneratedBase<ZeroForceControlVelocityCalcuation>(stateData)
{
}
 
void ZeroForceControlVelocityCalcuation::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    setTimeoutEvent(in.gettimeout(), createEventTimeout());
 
 
 
    ForceControlGroupStatechartContext* c = getContext<ForceControlGroupStatechartContext>();
    std::string tcpName;
 
    if (in.istcpNameSet())
    {
        tcpName = in.gettcpName();
    }
    else
    {
        tcpName = c->getRobot()->getRobotNodeSet(in.getrobotNodeSetName())->getTCP()->getName();
    }
 
    DatafieldRefPtr forceRef = in.getforceRef();
    DatafieldRefPtr torqueRef = in.gettorqueRef();
    // ARMARX_INFO << deactivateSpam(2) << "ForceRef: " <<  forceRef->getDataFieldIdentifier()->getIdentifierStr();
    Literal update(forceRef->getDataFieldIdentifier(), "updated", Literal::createParameterList());
    Literal update_torque(torqueRef->getDataFieldIdentifier(), "updated", Literal::createParameterList());
 
    installConditionForSensorUpdate(update && update_torque);
    auto now = TimeUtil::GetTime();
    IceUtil::Time duration;
 
    if (in.istimestampSet())
    {
        duration = now - in.gettimestamp()->toTime();
    }
    else
    {
        duration = IceUtil::Time::milliSecondsDouble(0);
    }
 
    //ARMARX_IMPORTANT << deactivateSpam(1) << "DURATION: " << duration.toMilliSecondsDouble();
    std::string robotNodeSetName = in.getrobotNodeSetName();
 
 
    //*************Force Calc****************
    FramedDirectionPtr vel = in.getcurrentVelocity();
    //    FramedDirectionPtr curAcc = in.getcurrentAcc();
    FramedDirectionPtr curForce = forceRef->getDataField()->get<FramedDirection>();
    vel = FramedDirection::ChangeFrame(c->getRobot(), *vel, curForce->frame);
    //    ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
    ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
    float forceThreshold = in.getforceThreshold();
    //float maxSensitivity = 2.0f;
 
    Eigen::Vector3f newVel(3);
    Eigen::Vector3f newAcc(3);
    ARMARX_INFO << deactivateSpam(1) << "force magnitude: " << curForce->toEigen().norm();
 
    if (curForce->toEigen().norm() > forceThreshold)
    {
        newAcc = curForce->toEigen() * in.getAccGain();
    }
    else if (vel->toEigen().norm() > 0)
    {
        ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
        newAcc = -in.getDecGain() * vel->toEigen().normalized();
    }
    else
    {
        newAcc = Eigen::Vector3f::Zero();
    }
 
    ARMARX_IMPORTANT << deactivateSpam(1) << "current curForce: " << curForce->output();
    ARMARX_IMPORTANT << deactivateSpam(1) << "current velocity: " << vel->output();
    newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble() * 0.001;
 
    if (newVel.norm() > in.getmaxVelocity())
    {
        newVel = newVel.normalized() * in.getmaxVelocity();
    }
 
    ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel: " << newVel;
    //    vel->x = 0;
    //    vel->y = 0;
 
    //*************Torque Calc****************
    FramedDirectionPtr velRot = in.getcurrentRotVelocity();
    //    FramedDirectionPtr curAcc = in.getcurrentRotAcc();
    FramedDirectionPtr curTorque = torqueRef->getDataField()->get<FramedDirection>();
    velRot = FramedDirection::ChangeFrame(c->getRobot(), *velRot, curTorque->frame);
    //    ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
    ARMARX_CHECK_EXPRESSION(velRot->frame == curTorque->frame);
    float torqueThreshold = in.gettorqueThreshold();
    //float maxSensitivity = 2.0f;
 
    Eigen::Vector3f newRotVel(3);
    Eigen::Vector3f newRotAcc(3);
 
    //ARMARX_INFO << deactivateSpam(1) << "force magnitude: " << curToruqe->toEigen().norm();
    if (curTorque->toEigen().norm() > torqueThreshold)
    {
        newRotAcc = curTorque->toEigen() * in.getRotAccGain();
    }
    else if (velRot->toEigen().norm() > 0)
    {
        //ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
        newRotAcc = -in.getRotDccGain() * velRot->toEigen().normalized();
    }
    else
    {
        newRotAcc = Eigen::Vector3f::Zero();
    }
 
    //ARMARX_IMPORTANT << deactivateSpam(1) << "current curTorque: " << curTorque->output();
    //ARMARX_IMPORTANT << deactivateSpam(1) << "current velocity: " << velRot->output();
    Eigen::Vector3f velRotDelta =  newRotAcc * duration.toMilliSecondsDouble() * 0.001;
 
    if (velRotDelta.norm() > in.getmaxRotAcc())
    {
        velRotDelta = velRotDelta.normalized() * in.getmaxRotAcc();
    }
 
    newRotVel = velRot->toEigen() + velRotDelta;
 
    if (newRotVel.norm() > in.getmaxVelocity())
    {
        newRotVel = newRotVel.normalized() * in.getmaxRotVelocity();
    }
 
    //   ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel: " << newRotVel;
    //    vel->x = 0;
    //    vel->y = 0;
 
 
 
 
    //*********************Output**********************
    out.settimestamp(new TimestampVariant(now));
    //out.setcurrentSensitivity(in.getcurrentSensitivity());
    out.setcurrentVelocity(new FramedDirection(newVel, vel->frame, vel->agent));
    out.setcurrentRotVelocity(new FramedDirection(newRotVel, velRot->frame, velRot->agent));
 
 
    FramedDirectionPtr newFramedVel = new FramedDirection(newVel, vel->frame, vel->agent);
    FramedDirectionPtr newFramedRotVel = new FramedDirection(newRotVel, velRot->frame, velRot->agent);
 
 
    newFramedVel = FramedDirection::ChangeFrame(c->getRobot(), *newFramedVel, c->getRobot()->getRootNode()->getName());
    ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel in platform: " << newFramedVel->output();
    newFramedRotVel = FramedDirection::ChangeFrame(c->getRobot(), *newFramedRotVel, c->getRobot()->getRootNode()->getName());
    ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel in platform: " << newFramedRotVel->output();
 
 
    c->getTCPControlUnit()->setTCPVelocity(robotNodeSetName, tcpName, newFramedVel,
                                           newFramedRotVel);
 
 
 
}
 
 
void ZeroForceControlVelocityCalcuation::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
 
}
 
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr ZeroForceControlVelocityCalcuation::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new ZeroForceControlVelocityCalcuation(stateData));
}