ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }