ActionPinchLoose.h

Go to the documentation of this file.

/*
 * Copyright (C) 2020 IIT-HHCM
 * Author: Davide Torielli
 * email:  davide.torielli@iit.it
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef __ROSEE_ACTIONPINCHLOOSE_H
#define __ROSEE_ACTIONPINCHLOOSE_H

#include <end_effector/GraspingActions/ActionPrimitive.h>
#include <end_effector/GraspingActions/ActionPinchGeneric.h>
#include <moveit/planning_scene/planning_scene.h>
#include <yaml-cpp/yaml.h>
#include <iostream>

namespace ROSEE {

class ActionPinchLoose : public ActionPinchGeneric
{
    
public:
    
    typedef std::map < std::pair<std::string, std::string>, ActionPinchLoose > Map;
    
    typedef std::pair <JointPos, double> StateWithDistance; 
    
    ActionPinchLoose();
    ActionPinchLoose ( unsigned int maxStoredActionStates );
    ActionPinchLoose ( std::string tip1, std::string tip2);
    ActionPinchLoose ( std::pair <std::string, std::string>, JointPos, double distance );
    
    JointPos getJointPos () const override;
    JointPos getJointPos (unsigned int index) const;
    
    std::vector < ROSEE::JointPos > getAllJointPos () const override;    
    
    std::vector < ROSEE::ActionPinchLoose::StateWithDistance > getActionStates() const;

    
    bool insertActionState (JointPos, double distance);

    /* For the pinch, we override these function to print, emit and parse the optional info Contact,
     which is specific of the pinch */
    void print () const override;
    void emitYaml ( YAML::Emitter&) const override;
    bool fillFromYaml( YAML::const_iterator yamlIt ) override;
    
private:
    
    struct distComp {
        bool operator() (const StateWithDistance& a, const StateWithDistance& b) const
        {return (std::abs(a.second) < std::abs(b.second) );}
    };
        
    std::set < StateWithDistance, distComp > actionStates;

};

}

#endif // __ROSEE_ACTIONPINCHLOOSE_H