PointingIK.cpp

Go to the documentation of this file.

#include "PointingIK.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
namespace armarx::control::pointing::core
{
 
    PointingIK::PointingIK(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f& target)
    {
        auto humanMapping = robot->getHumanMapping();
        ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
 
        auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
        auto elbow = armDescription.segments.elbow.nodeName;
        auto wrist = armDescription.segments.wrist.nodeName;
        auto palm = armDescription.segments.palm.nodeName;
 
        // actuated joints = all arm joints until (excluding) wrist
        auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
        auto actuatedJoints = nodeSetArm->getNodeNames();
        actuatedJoints.resize(nodeSetArm->getRobotNodeIndex(wrist));
 
        // reduced robot must contain shoulder, elbow, wrist and palm for IK to work
        reducedRobot_ = std::make_unique<simox::control::simox::robot::Robot>(
            robot, actuatedJoints, std::vector({wrist, palm}), true);
 
        ik_ = std::make_unique<simox::control::method::example::PointingIK>(
            *reducedRobot_,
            *reducedRobot_->getNode(wrist),
            *reducedRobot_->getNode(elbow),
            side == LEFT ? reducedRobot_->humanMapping()->leftArm
                         : reducedRobot_->humanMapping()->rightArm);
 
        ik_->setPointingTarget(target.cast<double>() / 1000); // simox uses meters
 
        trajData.assign(ik_->getNodeNames().size(), std::vector<double>());
        appendTrajData(ik_->getJointValues());
    }
 
    bool
    PointingIK::isTargetReached()
    {
        return ik_->isTargetReached();
    }
 
    double
    PointingIK::optimize()
    {
        Eigen::VectorXd prevJointValues = ik_->getJointValues();
        ik_->optimize();
        appendTrajData(ik_->getJointValues());
        return (ik_->getJointValues() - prevJointValues).norm();
    }
 
    TrajectoryPtr
    PointingIK::getTrajectory()
    {
        return new Trajectory(trajData, Ice::DoubleSeq(), ik_->getNodeNames());
    }
 
    void
    PointingIK::appendTrajData(const Eigen::VectorXd& jointValues)
    {
        for (size_t i = 0; i < trajData.size(); i++)
        {
            trajData[i].push_back(jointValues[i]);
        }
    }
 
} // namespace armarx::control::pointing::core