ARGoS  3
A parallel, multi-engine simulator for swarm robotics
pointmass3d_footbot_model.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/utility/math/cylinder.h>
9 
10 namespace argos {
11 
12  static const Real FOOTBOT_RADIUS = 0.085036758f;
13  static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
14  static const Real FOOTBOT_HEIGHT = 0.146899733f;
15 
19  };
20 
21  /****************************************/
22  /****************************************/
23 
25  CFootBotEntity& c_footbot) :
26  CPointMass3DModel(c_engine, c_footbot.GetEmbodiedEntity()),
27  m_cWheeledEntity(c_footbot.GetWheeledEntity()),
28  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
29  /* Register the origin anchor update method */
30  RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
32  /* Get initial rotation */
33  CRadians cTmp1, cTmp2;
35  }
36 
37  /****************************************/
38  /****************************************/
39 
42  CRadians cTmp1, cTmp2;
44  m_fAngularVelocity = 0.0;
45  }
46 
47  /****************************************/
48  /****************************************/
49 
50 
52  m_cVelocity.Set((m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] + m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL])*0.5, 0.0, 0.0);
53  m_cVelocity.RotateZ(m_cYaw);
54  m_fAngularVelocity = (m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] - m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL]) / FOOTBOT_INTERWHEEL_DISTANCE;
55  }
56 
57  /****************************************/
58  /****************************************/
59 
62  m_cYaw += CRadians(m_fAngularVelocity * m_cPM3DEngine.GetPhysicsClockTick());
63  }
64 
65  /****************************************/
66  /****************************************/
67 
70  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() - FOOTBOT_RADIUS,
71  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() - FOOTBOT_RADIUS,
72  GetEmbodiedEntity().GetOriginAnchor().Position.GetZ());
74  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() + FOOTBOT_RADIUS,
75  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() + FOOTBOT_RADIUS,
76  GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + FOOTBOT_HEIGHT);
77  }
78 
79  /****************************************/
80  /****************************************/
81 
83  const CRay3& c_ray) const {
84  CCylinder m_cShape(FOOTBOT_RADIUS,
85  FOOTBOT_HEIGHT,
87  CVector3::Z);
88  bool bIntersects = m_cShape.Intersects(f_t_on_ray, c_ray);
89  return bIntersects;
90  }
91 
92  /****************************************/
93  /****************************************/
94 
96  s_anchor.Position = m_cPosition;
97  s_anchor.Orientation = CQuaternion(m_cYaw, CVector3::Z);
98  }
99 
100  /****************************************/
101  /****************************************/
102 
104 
105  /****************************************/
106  /****************************************/
107 
108 }
argos::CVector3::Set
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
argos::CEmbodiedEntity::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition: embodied_entity.h:119
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::CPointMass3DModel::Reset
virtual void Reset()
Definition: pointmass3d_model.cpp:23
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CCylinder
Definition: cylinder.h:19
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
pointmass3d_footbot_model.h
argos::SBoundingBox::MinCorner
CVector3 MinCorner
Definition: physics_model.h:88
argos::SBoundingBox::MaxCorner
CVector3 MaxCorner
Definition: physics_model.h:89
argos::CRay3
Definition: ray3.h:19
argos::CVector3::Z
static const CVector3 Z
The z axis.
Definition: vector3.h:40
argos::FOOTBOT_LEFT_WHEEL
@ FOOTBOT_LEFT_WHEEL
Definition: dynamics2d_footbot_model.cpp:29
argos::CVector3::RotateZ
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:265
argos::CPointMass3DFootBotModel::Reset
virtual void Reset()
Definition: pointmass3d_footbot_model.cpp:40
argos::FOOTBOT_WHEELS
FOOTBOT_WHEELS
Definition: dynamics2d_footbot_model.cpp:28
argos::CPointMass3DModel
Definition: pointmass3d_model.h:21
argos::CFootBotEntity
Definition: footbot_entity.h:32
argos::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:171
argos::CQuaternion
Definition: quaternion.h:14
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CPointMass3DFootBotModel::CalculateBoundingBox
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
Definition: pointmass3d_footbot_model.cpp:68
argos::CCylinder::Intersects
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
Definition: cylinder.cpp:9
argos::CPhysicsModel::RegisterAnchorMethod
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
Definition: physics_model.h:325
argos::CPhysicsEngine::GetPhysicsClockTick
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
Definition: physics_engine.h:276
argos::CPointMass3DModel::m_cPM3DEngine
CPointMass3DEngine & m_cPM3DEngine
Reference to the physics engine.
Definition: pointmass3d_model.h:71
argos::CPointMass3DModel::m_cVelocity
CVector3 m_cVelocity
The linear velocity of this model in the engine.
Definition: pointmass3d_model.h:77
argos::CPhysicsModel::GetBoundingBox
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
Definition: physics_model.h:198
argos::CPointMass3DFootBotModel::CPointMass3DFootBotModel
CPointMass3DFootBotModel(CPointMass3DEngine &c_engine, CFootBotEntity &c_footbot)
Definition: pointmass3d_footbot_model.cpp:24
argos::CPointMass3DFootBotModel::UpdateOriginAnchor
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
Definition: pointmass3d_footbot_model.cpp:95
argos::CPointMass3DFootBotModel
Definition: pointmass3d_footbot_model.h:22
argos::FOOTBOT_RIGHT_WHEEL
@ FOOTBOT_RIGHT_WHEEL
Definition: dynamics2d_footbot_model.cpp:30
argos::CPointMass3DModel::m_cPosition
CVector3 m_cPosition
The position of the model in this engine.
Definition: pointmass3d_model.h:74
argos::CPointMass3DEngine
Definition: pointmass3d_engine.h:22
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::CPointMass3DFootBotModel::UpdateFromEntityStatus
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Definition: pointmass3d_footbot_model.cpp:51
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CPointMass3DFootBotModel::CheckIntersectionWithRay
virtual bool CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &c_ray) const
Definition: pointmass3d_footbot_model.cpp:82
argos::CPointMass3DFootBotModel::Step
virtual void Step()
Definition: pointmass3d_footbot_model.cpp:60
argos::REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY(CEyeBotEntity, CPointMass3DEyeBotModel)
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133