ActionPinchTight.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_ACTIONPINCHTIGHT_H
#define __ROSEE_ACTIONPINCHTIGHT_H

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

namespace ROSEE {

class ActionPinchTight : public ActionPinchGeneric
{
    
public:
    
    typedef std::map < std::pair<std::string, std::string>, ActionPinchTight > Map;

    
    typedef std::pair <JointPos, collision_detection::Contact> StateWithContact; 
    
    ActionPinchTight();
    ActionPinchTight(unsigned int maxStoredActionStates);
    ActionPinchTight (std::pair <std::string, std::string>, JointPos, collision_detection::Contact );
    ActionPinchTight (std::string finger1, std::string finger2, JointPos, collision_detection::Contact );
    
    JointPos getJointPos () const override;
    JointPos getJointPos (unsigned int index) const;
    
    std::vector < ROSEE::JointPos > getAllJointPos () const override;
    
    std::vector < ROSEE::ActionPinchTight::StateWithContact > getActionStates() const;
    
    bool insertActionState (JointPos, collision_detection::Contact);

    void print () const override;
    void emitYaml ( YAML::Emitter& ) const override;
    bool fillFromYaml( YAML::const_iterator yamlIt ) override;
    
private:
    
    bool emitYamlForContact ( collision_detection::Contact, YAML::Emitter& ) const;

    
    struct depthComp {
        bool operator() (const StateWithContact& a, const StateWithContact& b) const
        {return (std::abs(a.second.depth) > std::abs(b.second.depth) );}
    };
    
    std::set < StateWithContact, depthComp > actionStates;
    
};

}

#endif // __ROSEE_ACTIONPINCHTIGHT_H