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 WRAP_HPP
#define WRAP_HPP

#ifdef HPP

#include <string>
#include <vector>
#include <map>

#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/position.hh>
#include <hpp/constraints/orientation.hh>
#include <hpp/constraints/relative-com.hh>
#include <hpp/constraints/relative-position.hh>
#include <hpp/constraints/relative-orientation.hh>
#include <hpp/wholebody-step/fwd.hh>
#include <hpp/model/urdf/util.hh>
#include <hpp/core/manipulation-solver.hh>
#include <hpp/core/locked-dof.hh>
#include <hpp/core/path-vector.hh>

#include "configuration.hpp"
#include "graspPlanning/include/gpGrasp.h"
#include "p3d/proto/p3d_matrix_proto.h"
#include "API/Device/robot.hpp"
#include "Planner-pkg.h"


class Wrap {
public:
        Wrap();

        /* Load a 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 loadRobot(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);

        /* Lock a DoF
         *
         * \param jointName is the name of the DoF to lock
         * \param value is the value of the locked DoF
         */
        void addLockDof(const std::string& jointName, double value);

        /* Add a position constraints to the solver
         *
         * \param name is the full qualified and unique name of the constraints
         * \param jointName is the name of the joint to constrain
         * \param posLocal is the position of the joint in its own local frame
         * \param posGlobal is the position of the joint in the global frame
         * \param mask specifies which of the 3 coordinates are to be constrain
         */
        void addPositionConstraint(const std::string& name, const std::string& jointName,
                                                           hpp::model::vector3_t posLocal, hpp::model::vector3_t posGlobal,
                                                           std::vector<bool> mask);

        /* Add an orientation constraints to the solver
         *
         * \param name is the full qualified and unique name of the constraints
         * \param jointName is the name of the joint to constrain
         * \param rotation is the rotation matrix of the joint
         * \param mask specifies which of the 3 coordinates are to be constrain
         */
        void addOrientationConstraint(const std::string& name, const std::string& jointName,
                                                                  hpp::model::matrix3_t rotation, std::vector<bool> mask);

        /* Apply all constraints and find an IK solution
         *
         * \param config is both the input configuration and the updated output solution
         * \param timer is a pointer updated with the total time spent in solving the IK
         * \return whether a solution have been found or not
         */
        bool applyConstraints(hpp::core::ConfigurationPtr_t config, double *timer);

        /* Reset all the constraints
         */
        void resetConstraints();

        /* Update a position constraints already stored in the solver
         *
         * \param constraintName is the name of the constraints to update
         * \param globalValue is the new position of the joint in the global frame
         */
        void updatePositionConstraintRef(const std::string& constraintName, hpp::model::vector3_t globalValue);

        /* Update a position constraints already stored in the solver
         *
         * \param constraintName is the name of the constraints to update
         * \param localValue is the new position of the joint in its own local frame
         * \param globalValue is the new position of the joint in the global frame
         */
        void updatePositionConstraintRef(const std::string& constraintName, hpp::model::vector3_t localValue,
                                                                        hpp::model::vector3_t globalValue);

        /* Update an orientation constraints already stored in the solver
         *
         * \param constraintName is the name of the constraints to update
         * \param rot is the new rotation matrix of the joint
         */
        void updateOrientationConstraintRef(const std::string& constraintName, hpp::model::matrix3_t rot);

        /* 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;

        /* Find a trajectory
         *
         * \param initConf is the starting conf of the trajectory to plan
         * \param goalConf is the goal conf of the trajectory to plan
         * \param outputTraj is the solution trajectory planned
         * \param timer is the output duration of planing
         * \return whether a path has been found or not
         */
        bool findTrajectory(confPtr_t &initConf, confPtr_t &goalConf, p3d_traj *outputTraj, double *timer);



protected:
        /* Generate the open config for a grasp
         *
         * \param inputConf is the input configuration to be updated
         * \param outputConf is the output solution configuration with the open hand (fingers)
         * \return whether the config have been updated or not
         */
        bool generateOpenGraspConf(confPtr_t inputConf, confPtr_t outputConf);

        /* Generate the closed config for a grasp
         *
         * \param inputConf is the input configuration to be updated
         * \param outputConf is the output solution configuration with the closed hand (fingers)
         * \return whether the config have been updated or not
         */
        bool generateClosedGraspConf(confPtr_t inputConf, confPtr_t outputConf);

        /* Shoot a random config of the robot
         *
         * \param config is the output randomized config
         */
        void randomizeFullConfig(hpp::core::ConfigurationPtr_t config);

        /* Get the current HPP configuration of the robot
         *
         * \return the current configuration of the robot in HPP
         */
        hpp::core::ConfigurationPtr_t getCurrentConfig() {
                return boost::shared_ptr<hpp::model::Configuration_t> (new hpp::model::Configuration_t(_manipulationSolver->robot()->currentConfiguration()));
        }

        /* Set the current HPP configuration of the robot
         *
         * \param the configuration of the robot to be set in HPP
         */
        void setCurrentConfig(hpp::core::ConfigurationPtr_t conf) {
                _manipulationSolver->robot()->currentConfiguration(*conf);
        }

        /* Set bounds to a specific joint
         *
         * \param jointName is the name of the joint to bound
         * \param bounds is the sequence of bounds for each DoF of the joint
         */
        void setJointBound(std::string jointName, std::vector<double> bounds);

        /* Generate a Move3D configuration from an HPP configuration
         *
         * \param config is a sharedPtr to the input HPP config
         * \return a sharedPtr the corresponding Move3D config
         */
        confPtr_t createConfiguration(hpp::core::ConfigurationPtr_t config);

        /* Generate an HPP configuration from a Move3D configuration
         *
         * \param config is a sharedPtr to the input Move3D config
         * \return a sharedPtr the corresponding HPP config
         */
        hpp::core::ConfigurationPtr_t createHppConfiguration(confPtr_t q);

        /* Get the current value of a joint
         *
         * \param name is the name of the joint
         * \return the value of the current configuration of the joint
         */
        double getJointDofValue(const std::string& name);

        /* Create a Move3D trajectory from an HPP path
         *
         * \param inputPath is the HPP input localPath
         * \param outputTrajectory is the Move3D output trajectory generated
         * \return whether the trajectory has been created or not
         */
        bool createTrajectory(hpp::core::PathPtr_t inputPath, p3d_traj *outputTrajectory);

        char *_eef_joint;
        gpGrasp _grasp;
        Robot *_move3dRobot;

        hpp::core::ManipulationSolverPtr_t _manipulationSolver;
        hpp::core::ConfigurationPtr_t confSol;
};

#endif /* HPP */
#endif /* WRAP_HPP */