PointingIK.h

Go to the documentation of this file.

#pragma once
 
#include <RobotAPI/libraries/core/Trajectory.h>
 
#include "Side.h"
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/example/method/PointingIK.h>
 
namespace armarx::control::pointing::core
{
 
    /**
     * @brief IK to compute trajectory for a pointing gesture.
     *
     * Wrapper for simox-control's method::example::PointingIK. Computes a  trajectory for a
     * pointing gesture that lines up the forearm with a target. Uses the robot's human mapping to
     * obtain relevant joints: All arm-joints until (excluding) the wrist are being used.
     *
     * This class keeps an internal robot model. Each call to @ref optimize() optimizes the joint
     * configuration regarding pointing towards the target. @ref isTargetReached() returns whether
     * the robot in the latest configuration already points at the target. @ref getTrajectory()
     * returns the sequence of generated configurations as Trajectory.
     *
     * Example usage:
     *
     * @code
     * ik = PointingIK(...);
     *
     * while (not ik.isTargetReached()) {
     *    if (ik.optimize) < 1e-6 {
     *        return nullptr;
     *    }
     * }
     * return ik.getTrajectory();
     * @endcode
     *
     * @ingroup Library-pointing
     */
    class PointingIK
    {
    public:
        /**
         * @brief Initialize a new PointingIK.
         * @param robot Robot model with current robot configuration to be used by the IK. Must
         *        provide a human mapping.
         * @param side Side of the arm to point with.
         * @param target Target position to point at in the global coordinate system (mm).
         */
        PointingIK(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f& target);
 
        /**
         * @return Whether the robot in the latest joint configuration (from @ref optimize())
         * already points at the target.
         */
        bool isTargetReached();
 
        /**
         * @brief Optimizes the joint configuration of the internal robot model regarding pointing
         *        towards the target.
         * @return Difference between the previous and new joint configurations. May be used to
         *         determine if the IK is unable to find a solution.
         */
        double optimize();
 
        /**
         * @brief Constructs a trajectory containing the sequence of joint configurations from the
         *        calls to @ref optimize().
         *
         * Neither the trajectory nor the end configuration are guaranteed to be self-
         * collision free. Furthermore, the trajectories aren't efficient in reaching the end
         * configuration.
         *
         * `getTrajectory()->size()` will equal the number of calls to @ref optimize() plus one for
         * the initial joint configuration
         *
         * @return The constructed trajectory.
         */
        armarx::TrajectoryPtr getTrajectory();
 
    private:
        std::unique_ptr<simox::control::simox::robot::Robot> reducedRobot_;
        std::unique_ptr<simox::control::method::example::PointingIK> ik_;
 
        std::vector<std::vector<double>> trajData; // actuated joints x timesteps
        void appendTrajData(const Eigen::VectorXd& jointValues);
    };
 
} // namespace armarx::control::pointing::core