ActionMultiplePinchTight.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_ACTIONMULTIPLEPINCHTIGHT_H
#define __ROSEE_ACTIONMULTIPLEPINCHTIGHT_H

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

namespace ROSEE {

class ActionMultiplePinchTight : public ActionPinchGeneric
{
    
public:
    
    typedef std::map < std::set<std::string>, ActionMultiplePinchTight > Map;
    
    typedef std::pair <JointPos, double> StateWithDepth; 
    
    ActionMultiplePinchTight();
    ActionMultiplePinchTight(unsigned int maxStoredActionStates);
    ActionMultiplePinchTight (std::set <std::string>, JointPos, double depthSum );
    
    JointPos getJointPos () const override;
    JointPos getJointPos (unsigned int index) const;
    
    std::vector < ROSEE::JointPos > getAllJointPos () const override;
    
    std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > getActionStates() const;
    
    bool insertActionState (JointPos, double depthSum);

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

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

}

#endif // __ROSEE_ACTIONMULTIPLEPINCHTIGHT_H