ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/gripper_equipped_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "gripper_equipped_entity.h"
00008 #include <argos3/core/simulator/space/space.h>
00009 
00010 namespace argos {
00011    
00012    /****************************************/
00013    /****************************************/
00014 
00015    CRange<Real> UNIT(0.0f, 1.0f);
00016 
00017    /****************************************/
00018    /****************************************/
00019 
00020    CGripperEquippedEntity::CGripperEquippedEntity(CComposableEntity* pc_parent) :
00021       CEntity(pc_parent),
00022       m_fLockState(0.0f),
00023       m_fLockThreshold(0.5f),
00024       m_pcGrippedEntity(NULL) {
00025       Disable();
00026       SetCanBeEnabledIfDisabled(false);
00027    }
00028          
00029    /****************************************/
00030    /****************************************/
00031          
00032    CGripperEquippedEntity::CGripperEquippedEntity(CComposableEntity* pc_parent,
00033                                                   const std::string& str_id,
00034                                                   const CVector3& c_offset,
00035                                                   const CVector3& c_direction,
00036                                                   Real f_lock_threshold) :
00037       CEntity(pc_parent,
00038               str_id),
00039       m_cOffset(c_offset),
00040       m_cInitOffset(c_offset),
00041       m_cDirection(c_direction),
00042       m_cInitDirection(c_direction),
00043       m_fLockState(0.0f),
00044       m_fLockThreshold(f_lock_threshold),
00045       m_pcGrippedEntity(NULL) {
00046       Disable();
00047       SetCanBeEnabledIfDisabled(false);
00048    }
00049          
00050    /****************************************/
00051    /****************************************/
00052 
00053    void CGripperEquippedEntity::Init(TConfigurationNode& t_tree) {
00054       try {
00055          /* Parse id */
00056          CEntity::Init(t_tree);
00057          /* Parse gripper attributes */
00058          GetNodeAttribute(t_tree, "offset", m_cOffset);
00059          m_cInitOffset = m_cOffset;
00060          GetNodeAttribute(t_tree, "direction", m_cDirection);
00061          m_cInitDirection = m_cDirection;
00062          GetNodeAttributeOrDefault(t_tree, "lock_threshold", m_fLockThreshold, m_fLockThreshold);
00063       }
00064       catch(CARGoSException& ex) {
00065          THROW_ARGOSEXCEPTION_NESTED("Error while initializing the gripper entity \"" << GetId() << "\"", ex);
00066       }
00067    }
00068          
00069    /****************************************/
00070    /****************************************/
00071 
00072    void CGripperEquippedEntity::Reset() {
00073       m_fLockState = 0.0f;
00074       m_cOffset = m_cInitOffset;
00075       m_cDirection = m_cInitDirection;
00076       ClearGrippedEntity();
00077    }
00078 
00079    /****************************************/
00080    /****************************************/
00081          
00082    void CGripperEquippedEntity::SetLockState(Real f_lock_state) {
00083       UNIT.TruncValue(f_lock_state);
00084       m_fLockState = f_lock_state;
00085    }
00086 
00087    /****************************************/
00088    /****************************************/
00089          
00090    void CGripperEquippedEntity::SetLockThreshold(Real f_lock_threshold) {
00091       UNIT.TruncValue(f_lock_threshold);
00092       m_fLockThreshold = f_lock_threshold;
00093    }
00094 
00095    /****************************************/
00096    /****************************************/
00097          
00098    CEmbodiedEntity& CGripperEquippedEntity::GetGrippedEntity() {
00099       if(m_pcGrippedEntity != NULL) {
00100          return *m_pcGrippedEntity;
00101       }
00102       else {
00103          THROW_ARGOSEXCEPTION("Entity \"" << GetId() << "\" is not gripping anything.");
00104       }
00105    }
00106 
00107    /****************************************/
00108    /****************************************/
00109          
00110    REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CGripperEquippedEntity);
00111 
00112    /****************************************/
00113    /****************************************/
00114 
00115 }