Humanoid_Wrap.hpp

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

#ifndef HUMANOID_WRAP_HPP
#define HUMANOID_WRAP_HPP

#ifdef HPP

#include "Wrap.hpp"

class HumanoidWrap : public Wrap {
public:
        HumanoidWrap();
        /* Load a Humanoid Robot model in HPP from URDF/SRDF
         *
         * \param robotName is the name of the robot in HPP
         * \param rootJointType is the type of rootJoint of the robot (freeflyer, planar, anchor, ...)
         * \param packageName is the name of the ros package containing the robot description
         * \param modelName is the name of the robot model
         * \param urdfSuffix is a suffix to the URDF file name
         * \param srdfSuffix is a suffix to the SRDF file name
         * \param move3dRobot is the corresponding robot strucutre in Move3D
         * \return a boolean value whether the model was correctly loaded in HPP or not
         */

        bool loadHumanoidRobot(const std::string& robotName, const std::string& rootJointType,
                                                   const std::string& packageName, const std::string& modelName,
                                                   const std::string& urdfSuffix, const std::string& srdfSuffix,
                                                   Robot *move3dRobot);

        /* Virtual method to generate 1 single configuration, specific to each robot */
        virtual bool generateConfig(confPtr_t &inputConf, confPtr_t &outputConf, p3d_matrix4 tragetPos, int armId, bool randomize) = 0;
        /* Virtual method to generate 4 grasp configurations, specific to each robot */
        virtual std::vector<confPtr_t> generateGraspConfigs(confPtr_t &q, gpGrasp grasp, p3d_matrix4 objectPos,
                                                                                                                p3d_vector3 approachDir, double approachOffset,
                                                                                                                p3d_vector3 extractDir, double extractOffset) = 0;

        /* Create stability constraints specific to Humanoid
         */
        void createStabilityConstraints();

        /* Check if an input configuration is stable or not
         *
         * \param configuration is the input config to test
         * \return a boolean value to specify whether the config is stable or not
         */
        bool isPositionStable(confPtr_t config);

protected:
        std::map < std::string, double > startConf;
        std::string _leftAnkle;
        std::string _rightAnkle;

        std::vector<hpp::core::DifferentiableFunctionPtr_t> _numericalConstraints;
};

#endif /* HPP */
#endif /* HUMANOID_WRAP_HPP */