CalcVelocities.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::FindAndGraspObjectGroup
 * @author     Valerij Wittenbeck ( valerij dot wittenbeck at student dot kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalcVelocities.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
 
using namespace armarx;
using namespace FindAndGraspObjectGroup;
 
// DO NOT EDIT NEXT LINE
CalcVelocities::SubClassRegistry CalcVelocities::Registry(CalcVelocities::GetName(),
                                                          &CalcVelocities::CreateInstance);
 
CalcVelocities::CalcVelocities(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CalcVelocities>(stateData)
{
}
 
void
CalcVelocities::onEnter()
{
    // todo: set this via a configuration file
    ControlMode controlMode = ePositionControl;
    ARMARX_DEBUG << "Entering CalcVelocities::onEnter()";
 
    // TODOS here:
    // CRITICAL:
    // * marker to TCP offset (need name as datafield in object channel)
    // * use relative Target (Done, but still debugging...)
    // OPTIONAL:
    // * forces
 
    FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
 
    // retrieve channels
    //ChannelRefPtr objectChannel = getInput<ChannelRef>("objectChannel");    //not needed anymore
    ChannelRefPtr markerChannel = getInput<ChannelRef>("markerChannel");
 
    VirtualRobot::RobotPtr localRobot = context->localRobot;
    RemoteRobot::synchronizeLocalClone(localRobot, context->robotStateComponent);
 
    // use robot localization
    //PosePtr globalRobotPose = PosePtr::dynamicCast(context->simulatorProxy->getRobotPose(localRobot->getName()));
    //localRobot->setGlobalPose(globalRobotPose->toEigen());
    /*
        float lo,hi,jv;
        // SIMULATOR
        ARMARX_INFO << "------------------ SIM -----------------------" << flush;
        lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Elbow R");
        hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Elbow R");
        jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Elbow R");
        ARMARX_INFO << "Elbow R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
        lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Underarm R");
        hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Underarm R");
        jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Underarm R");
        ARMARX_INFO << "Underarm R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
        lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Wrist 1 R");
        hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Wrist 1 R");
        jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Wrist 1 R");
        ARMARX_INFO << "Wrist 1 R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
        lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Wrist 2 R");
        hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Wrist 2 R");
        jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Wrist 2 R");
        ARMARX_INFO << "Wrist 2 R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        PosePtr rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Elbow R"));
        Eigen::Vector3f gpPos = rnPose->toEigen().block(0,3,3,1);
        ARMARX_INFO << "Elbow R (SIM): gp:" << gpPos.transpose() << flush;
        rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Underarm R"));
        gpPos = rnPose->toEigen().block(0,3,3,1);
        ARMARX_INFO << "Underarm R (SIM): gp:" << gpPos.transpose() << flush;
        rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Wrist 1 R"));
        gpPos = rnPose->toEigen().block(0,3,3,1);
        ARMARX_INFO << "Wrist 1 R (SIM): gp:" << gpPos.transpose() << flush;
        rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Wrist 2 R"));
        gpPos = rnPose->toEigen().block(0,3,3,1);
        ARMARX_INFO << "Wrist 2 R (SIM): gp:" << gpPos.transpose() << flush;
 
 
        // REMOTE ROBOT
        ARMARX_INFO << "------------------ REMOTE ROB -----------------------" << flush;
        armarx::SharedRobotNodeInterfacePrx rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Elbow R");
        lo = rn->getJointLimitLow();
        hi = rn->getJointLimitHigh();
        jv = rn->getJointValue();
        ARMARX_INFO << "Elbow R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Underarm R");
        lo = rn->getJointLimitLow();
        hi = rn->getJointLimitHigh();
        jv = rn->getJointValue();
        ARMARX_INFO << "Underarm R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Wrist 1 R");
        lo = rn->getJointLimitLow();
        hi = rn->getJointLimitHigh();
        jv = rn->getJointValue();
        ARMARX_INFO << "Wrist 1 R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Wrist 2 R");
        lo = rn->getJointLimitLow();
        hi = rn->getJointLimitHigh();
        jv = rn->getJointValue();
        ARMARX_INFO << "Wrist 2 R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
 
        // CLONE local
         ARMARX_INFO << "------------------ LOCAL ROB CLONE -----------------------" << flush;
        lo = localRobot->getRobotNode("Elbow R")->getJointLimitLo();
        hi = localRobot->getRobotNode("Elbow R")->getJointLimitHi();
        jv = localRobot->getRobotNode("Elbow R")->getJointValue();
        ARMARX_INFO << "Elbow R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        lo = localRobot->getRobotNode("Underarm R")->getJointLimitLo();
        hi = localRobot->getRobotNode("Underarm R")->getJointLimitHi();
        jv = localRobot->getRobotNode("Underarm R")->getJointValue();
        ARMARX_INFO << "Underarm R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        lo = localRobot->getRobotNode("Wrist 1 R")->getJointLimitLo();
        hi = localRobot->getRobotNode("Wrist 1 R")->getJointLimitHi();
        jv = localRobot->getRobotNode("Wrist 1 R")->getJointValue();
        ARMARX_INFO << "Wrist 1 R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        lo = localRobot->getRobotNode("Wrist 2 R")->getJointLimitLo();
        hi = localRobot->getRobotNode("Wrist 2 R")->getJointLimitHi();
        jv = localRobot->getRobotNode("Wrist 2 R")->getJointValue();
        ARMARX_INFO << "Wrist 2 R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
 
        gpPos = localRobot->getRobotNode("Elbow R")->getGlobalPose().block(0,3,3,1);
        ARMARX_INFO << "Elbow R (local): gp:" << gpPos.transpose() << flush;
        gpPos = localRobot->getRobotNode("Underarm R")->getGlobalPose().block(0,3,3,1);
        ARMARX_INFO << "Underarm R (local): gp:" << gpPos.transpose() << flush;
        gpPos = localRobot->getRobotNode("Wrist 1 R")->getGlobalPose().block(0,3,3,1);
        ARMARX_INFO << "Wrist 1 R (local): gp:" << gpPos.transpose() << flush;
        gpPos = localRobot->getRobotNode("Wrist 2 R")->getGlobalPose().block(0,3,3,1);
        ARMARX_INFO << "Wrist 2 R (local): gp:" << gpPos.transpose() << flush;
    */
 
    // retrieve Jacobian pseudo inverse for TCP and chain
    std::string robotNodeSetName = getInput<std::string>("kinematicChain");
    std::string tcpNodeName = getInput<std::string>("tcpName");
    ARMARX_DEBUG << "robot node set: " << robotNodeSetName << ", tcp: " << tcpNodeName;
 
    VirtualRobot::RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(robotNodeSetName);
    VirtualRobot::RobotNodePtr tcpNode = localRobot->getRobotNode(tcpNodeName);
    //Eigen::Matrix4f tcpGlobalPose = tcpNode->getGlobalPose();
 
    // use the current tcp pose from the simulator -> there seems to be a slight error between the kinematic model and the bullet simulation (constraint violations!!)
    // use RobotHandLocalizerDynamicSimulation
    PosePtr rnPose = PosePtr::dynamicCast(
        context->simulatorProxy->getRobotNodePose(localRobot->getName(), tcpNodeName));
    Eigen::Matrix4f tcpGlobalPose = rnPose->toEigen();
 
 
    //VSContext->simulatorProxy->visualizePose("tcpGlobalPose", new FramedPose(tcpGlobalPose, ""));
    //VSContext->simulatorProxy->visualizePose("Wrist 2 R", new FramedPose(localRobot->getRobotNode("Wrist 2 R")->getGlobalPose(), ""));
    //VSContext->simulatorProxy->visualizePose("Wrist 1 R", new FramedPose(localRobot->getRobotNode("Wrist 1 R")->getGlobalPose(), ""));
    //VSContext->simulatorProxy->visualizePose("Underarm R", new FramedPose(localRobot->getRobotNode("Underarm R")->getGlobalPose(), ""));
    //VSContext->simulatorProxy->visualizePose("Elbow R", new FramedPose(localRobot->getRobotNode("Elbow R")->getGlobalPose(), ""));
    //VSContext->simulatorProxy->visualizePose("Shoulder 1 R", new FramedPose(localRobot->getRobotNode("Shoulder 1 R")->getGlobalPose(), ""));
    /*
        FramedPositionPtr mPos = markerChannel->get<FramedPosition>("position");
        FramedOrientationPtr mOrient= markerChannel->get<FramedOrientation>("orientation");
        Eigen::Matrix4f markerLocalPose = Eigen::Matrix4f::Identity();
        markerLocalPose.block(0,3,3,1) = mPos->toEigen();
        markerLocalPose.block(0,0,3,3) = mOrient->toEigen();*/
 
    //        Eigen::Matrix4f markerGlobalPose = localRobot->getRobotNode(mPos->getFrame())->getGlobalPose() * markerLocalPose;
    //        VSContext->simulatorProxy->visualizePose("markerGlobalPose", new FramedPose(markerGlobalPose, ""));
 
    //OLD
    /*
    // retrieve pose of object
    Eigen::Matrix4f objectPose_ref = getObjectPoseInRefFrame(objectChannel, refFrame, VSContext);
    ARMARX_VERBOSE << "Pose of object in reference frame " << objectPose_ref;
    */
 
    //NEW:
    //relative target pose, computed in VisualServoObject::onEnter()
    FramedPosePtr targetFramedPose = getInput<FramedPose>("targetPose");
    Eigen::Matrix4f targetPose = targetFramedPose->toEigen();
 
    Eigen::Matrix4f targetGlobalPose = localRobot->getRobotNode(targetFramedPose->getFrame())
                                           ->toGlobalCoordinateSystem(targetPose);
 
    // used debug drawer!
    //context->simulatorProxy->visualizePose("targetGlobalPose", new FramedPose(targetGlobalPose, ""));
    //context->simulatorProxy->visualizePose("tcpGlobalPose", new FramedPose(tcpGlobalPose, ""));
 
    // retrieve reference frame
    //std::string refFrame = getInput<std::string>("referenceFrame");
    std::string refFrame = targetFramedPose->getFrame();
    ARMARX_VERBOSE << "Using Reference Frame " << refFrame;
 
 
    // retrieve pose of marker
    //        Eigen::Matrix4f markerPose_ref = getObjectPoseInRefFrame(markerChannel, refFrame, VSContext);
    //        ARMARX_VERBOSE << "Pose of marker in reference frame " << markerPose_ref;
 
    Eigen::VectorXf errorCartVec(6);
    ARMARX_DEBUG << "target: " << targetGlobalPose.block(0, 3, 3, 1).transpose();
    ARMARX_DEBUG << "tcp: " << tcpGlobalPose.block(0, 3, 3, 1).transpose();
    errorCartVec.block(0, 0, 3, 1) =
        targetGlobalPose.block(0, 3, 3, 1) - tcpGlobalPose.block(0, 3, 3, 1);
    //        errorCartVec(2) += 180.0f;
 
    Eigen::Matrix4f orientation = targetGlobalPose * tcpGlobalPose.inverse();
    Eigen::AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
    errorCartVec.tail(3) = aa.axis() * aa.angle();
 
    ARMARX_DEBUG << "Cartesian error (RAW)" << errorCartVec.transpose();
 
 
    float distance = errorCartVec.head(3).norm();
 
    ARMARX_DEBUG << "Distance: " << distance << flush;
 
    if (distance < 10.0f)
    {
        EventPtr reached = new EvTargetReached(EVENTTOALL);
        sendEvent(reached);
    }
 
 
    float stepSize = getInput<float>("stepSize");
    errorCartVec = errorCartVec * stepSize;
 
    float IKCutCartErrorMM = getInput<float>("IKCutCartErrorMM");
 
    if (errorCartVec.head(3).norm() > IKCutCartErrorMM)
    {
        errorCartVec = errorCartVec / errorCartVec.head(3).norm() * IKCutCartErrorMM;
    }
 
 
    ARMARX_DEBUG << "Cartesian error (SCALED, cut:" << IKCutCartErrorMM << " mm)"
                 << errorCartVec.transpose();
 
    bool useOrientation = getInput<bool>("useOrientation");
 
    //        VirtualRobot::DifferentialIK dIK(robotNodeSet,localRobot->getRobotNode(refFrame));
    VirtualRobot::DifferentialIK dIK(robotNodeSet);
 
    Eigen::VectorXf errorJoint(robotNodeSet->getSize());
 
    IceUtil::Time startTime;
    IceUtil::Time endTime;
    clock_t startT;
    clock_t endT;
 
    if (useOrientation)
    {
        startTime = IceUtil::Time::now();
        startT = clock();
        Eigen::MatrixXf Ji =
            dIK.getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::All);
        endT = clock();
        endTime = IceUtil::Time::now();
        ARMARX_DEBUG << "Jacobian (Orientation) " << Ji;
 
        // calculate joint error
        errorJoint = Ji * errorCartVec;
    }
    else
    {
        startTime = IceUtil::Time::now();
        startT = clock();
 
        Eigen::MatrixXf Ji =
            dIK.getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::Position);
 
        endT = clock();
        endTime = IceUtil::Time::now();
 
        ARMARX_DEBUG << "Jacobian (Only Position)" << Ji;
 
        // calculate joint error
        Eigen::VectorXf errorPosition(3);
        errorPosition(0) = errorCartVec(0);
        errorPosition(1) = errorCartVec(1);
        errorPosition(2) = errorCartVec(2);
 
        ARMARX_DEBUG << "Position error " << errorPosition;
 
        errorJoint = Ji * errorPosition;
    }
 
    IceUtil::Time interval = endTime - startTime;
 
    if (interval.toMilliSeconds() > 80.0)
    {
        float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
        ARMARX_WARNING << "Jacobi calculation time:" << interval.toMilliSeconds()
                       << " ms (wall time!). Pure CPU time:" << diffClock << "ms";
    }
 
    float VEL_GAIN = 1.0f;
    float VEL_MAX_COEFF = 1.0f; //0.02f;
    ARMARX_DEBUG << "JointVelocities " << errorJoint.transpose();
    ARMARX_DEBUG << "JointVelocities GAIN" << errorJoint.transpose() * VEL_GAIN;
    float maxV = fabs(errorJoint.maxCoeff());
 
    if (maxV > VEL_MAX_COEFF)
    {
        errorJoint = errorJoint / maxV * VEL_MAX_COEFF;
        ARMARX_DEBUG << "JointVelocities VEL_MAX_COEFF" << errorJoint.transpose() * VEL_GAIN;
    }
 
 
    // build name value map
    NameValueMap targetVelocities;
    NameValueMap targetAngles;
    NameControlModeMap controlModes;
    const std::vector<VirtualRobot::RobotNodePtr> nodes = robotNodeSet->getAllRobotNodes();
 
    for (size_t i = 0; i < nodes.size(); i++)
    {
        VirtualRobot::RobotNodePtr n = nodes[i];
        targetVelocities[n->getName()] = errorJoint(i) * VEL_GAIN;
        targetAngles[n->getName()] = n->getJointValue() + errorJoint(i);
        controlModes[n->getName()] = controlMode; //ePositionVelocityControl;
        //controlModes[n->getName()] = eVelocityControl;
    }
 
    // execute velocities
    switch (controlMode)
    {
        case ePositionControl:
            ARMARX_DEBUG << "Target Angles: " << targetAngles;
            context->kinematicUnitPrx->switchControlMode(controlModes);
            context->kinematicUnitPrx->setJointAngles(targetAngles);
            break;
 
        case eVelocityControl:
            context->kinematicUnitPrx->switchControlMode(controlModes);
            context->kinematicUnitPrx->setJointVelocities(targetVelocities);
            break;
 
        case ePositionVelocityControl:
            context->kinematicUnitPrx->switchControlMode(controlModes);
            context->kinematicUnitPrx->setJointAngles(targetAngles);
            context->kinematicUnitPrx->setJointVelocities(targetVelocities);
            break;
 
        default:
            ARMARX_ERROR << "control mode nyi..." << flush;
            break;
    }
 
 
    //VSContext->kinematicUnitPrx->setJoints(targetAngles, targetVelocities);
 
    // for now we directly use the simulator to actuate the joints, should be done via the KinematicUnitDynamicsSimulation interface (must implement pos+vel control)!
    //VSContext->simulatorProxy->actuateRobotJoints(localRobot->getName(), targetAngles, targetVelocities);
    sendEvent<EvCalcVelocitiesDone>();
    ARMARX_VERBOSE << "Done CalcVelocities::onEnter()";
}
 
void
CalcVelocities::onExit()
{
}
 
Eigen::Matrix4f
getObjectPoseInRefFrame(ChannelRefPtr channel,
                        const std::string& refFrame,
                        FindAndGraspObjectContext* context)
{
    FramedPositionPtr position = channel->get<FramedPosition>("position");
    FramedOrientationPtr orientation = channel->get<FramedOrientation>("orientation");
 
    VirtualRobot::LinkedCoordinate markerPose =
        FramedPose::createLinkedCoordinate(context->remoteRobot, position, orientation);
 
    return markerPose.getInFrame(refFrame);
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
CalcVelocities::GetName()
{
    return "CalcVelocities";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CalcVelocities::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CalcVelocities(stateData));
}