Romeo_Wrap.hpp

/*
 * Romeo_Wrap.hpp : Wrapper between Move3D and HPP that provides an interface for handling Romeo
 *
 * Author : renaud Viry (renaud.viry@laas.fr)
 * Copyright LAAS-CNRS 2014
 *
 */

#ifndef ROMEO_WRAP_HPP
#define ROMEO_WRAP_HPP

#ifdef HPP

#include "Humanoid_Wrap.hpp"


class RomeoWrap : public HumanoidWrap {
public:
        RomeoWrap(Robot *move3dRobot);

        /* Set / Unset foot position in the stability plane (X, Y)
         *
         * \param fix boolean to fix / unfix
         */
        void fixFoot(bool fix) { _footFixed = fix; }

        /* Compute IK and generate a solution configuration for Romeo
         *
         * \param inputConf is a sharedPtr to the input configuration for the solver
         * \param outputConf is a sharedPtr to the solution output configuration
         * \param targetPos is the transformation Matrix for the target of the eef
         * \param armId is the if for the arm to plan (default = right, 1 = left)
         * \param randomize to specify if a randomization of initial config is required
         * \return whether a solution have been found or not
         */
        virtual bool generateConfig(confPtr_t &inputConf, confPtr_t &outputConf, p3d_matrix4 tragetPos, int armId, bool randomize);

        /* Compute IK and generate 4 solution configurations for grasp by Romeo
         *
         * \param q is a sharedPtr to the input configuration for the solver
         * \param grasp is the target grasp
         * \param objectPos is the transformation Matrix for the targeted  object of the grasp
         * \param approachDir is the approach direction of the grasp by the eef
         * \param approachOffset is the offset of the approach config
         * \param escapeDir is the escape direction after the grasp by the eef
         * \param escapeOffset is the offset of the escape config
         * \return the order solution configurations vector (approach, open, close, escape)
         */
        virtual std::vector<confPtr_t> generateGraspConfigs(confPtr_t &q, gpGrasp grasp, p3d_matrix4 objectPos,
                                                                                                                p3d_vector3 approachDir, double approachOffset,
                                                                                                                p3d_vector3 extractDir, double extractOffset);

private:
        /* Initialize basic and constant constraints*/
        void initializeConstraints();

        /* Randomize input config of the IK solver
         *
         * \param config is a shared pointer to the input configuration to randomize
         * \param randomizeParts is the map (name, bool value) describing which part to randomize
         */
        void randomizeConfig(hpp::core::ConfigurationPtr_t config, std::map<string, bool> randomizeParts);

        /* Get the starting (half-sitting) conf for Romeo
         *
         * \return the map (name, double value) of the joints
         */
        std::map < std::string, double > getStartConf();

        bool _footFixed;
};

#endif /* HPP */
#endif /* ROMEO_WRAP_HPP */