ARGoS  3
A parallel, multi-engine simulator for swarm robotics
dynamics2d_velocity_control.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
16  Real f_max_force,
17  Real f_max_torque) :
18  m_cDyn2DEngine(c_engine),
19  m_ptControlBody(NULL),
20  m_ptControlledBody(NULL),
21  m_ptLinearConstraint(NULL),
22  m_ptAngularConstraint(NULL),
23  m_fMaxForce(f_max_force),
24  m_fMaxTorque(f_max_torque) {
25  m_ptControlBody = cpBodyNew(INFINITY, INFINITY);
26  }
27 
28  /****************************************/
29  /****************************************/
30 
32  cpBodyFree(m_ptControlBody);
33  }
34 
35  /****************************************/
36  /****************************************/
37 
38  void CDynamics2DVelocityControl::AttachTo(cpBody* pt_body) {
39  /* If we are already controlling a body, detach from it first */
40  if(m_ptControlledBody != NULL) {
41  Detach();
42  }
43  /* Set the wanted body as the new controlled one */
44  m_ptControlledBody = pt_body;
45  /* Add linear constraint */
47  cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
48  cpPivotJointNew2(m_ptControlledBody,
50  cpvzero,
51  cpvzero));
52  m_ptLinearConstraint->maxBias = 0.0f; /* disable joint correction */
53  m_ptLinearConstraint->maxForce = m_fMaxForce; /* limit the dragging force */
54  /* Add angular constraint */
56  cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
57  cpGearJointNew(m_ptControlledBody,
59  0.0f,
60  1.0f));
61  m_ptAngularConstraint->maxBias = 0.0f; /* disable joint correction */
62  m_ptAngularConstraint->maxForce = m_fMaxTorque; /* limit the torque */
63  }
64 
65  /****************************************/
66  /****************************************/
67 
69  if(m_ptControlledBody != NULL) {
70  /* Remove constraints */
71  cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptLinearConstraint);
72  cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptAngularConstraint);
73  /* Free memory up */
74  cpConstraintFree(m_ptLinearConstraint);
75  cpConstraintFree(m_ptAngularConstraint);
76  /* Erase pointer to controlled body */
77  m_ptControlledBody = NULL;
78  }
79  }
80 
81  /****************************************/
82  /****************************************/
83 
85  m_ptControlBody->v.x = 0;
86  m_ptControlBody->v.y = 0;
87  m_ptControlBody->w = 0;
88  }
89 
90  /****************************************/
91  /****************************************/
92 
94  return CVector2(m_ptControlledBody->v.x,
95  m_ptControlledBody->v.y);
96  }
97 
98  /****************************************/
99  /****************************************/
100 
102  m_ptControlBody->v.x = c_velocity.GetX();
103  m_ptControlBody->v.y = c_velocity.GetY();
104  }
105 
106  /****************************************/
107  /****************************************/
108 
110  return m_ptControlBody->w;
111  }
112 
113  /****************************************/
114  /****************************************/
115 
117  m_ptControlBody->w = f_velocity;
118  }
119 
120  /****************************************/
121  /****************************************/
122 
123 }
argos::CDynamics2DVelocityControl::~CDynamics2DVelocityControl
virtual ~CDynamics2DVelocityControl()
Definition: dynamics2d_velocity_control.cpp:31
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CDynamics2DEngine
Definition: dynamics2d_engine.h:42
dynamics2d_velocity_control.h
argos::CVector2::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
argos::CDynamics2DVelocityControl::Reset
void Reset()
Definition: dynamics2d_velocity_control.cpp:84
argos::CDynamics2DVelocityControl::GetLinearVelocity
CVector2 GetLinearVelocity() const
Definition: dynamics2d_velocity_control.cpp:93
argos::CDynamics2DVelocityControl::SetLinearVelocity
void SetLinearVelocity(const CVector2 &c_velocity)
Definition: dynamics2d_velocity_control.cpp:101
argos::CDynamics2DVelocityControl::m_ptLinearConstraint
cpConstraint * m_ptLinearConstraint
Definition: dynamics2d_velocity_control.h:59
argos::CDynamics2DEngine::GetPhysicsSpace
cpSpace * GetPhysicsSpace()
Definition: dynamics2d_engine.h:73
argos::CDynamics2DVelocityControl::Detach
void Detach()
Definition: dynamics2d_velocity_control.cpp:68
argos::CVector2::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
argos::CDynamics2DVelocityControl::m_ptControlledBody
cpBody * m_ptControlledBody
Definition: dynamics2d_velocity_control.h:57
argos::CDynamics2DVelocityControl::m_fMaxTorque
Real m_fMaxTorque
Definition: dynamics2d_velocity_control.h:65
argos::CVector2
A 2D vector class.
Definition: vector2.h:25
argos::CDynamics2DVelocityControl::GetAngularVelocity
Real GetAngularVelocity() const
Definition: dynamics2d_velocity_control.cpp:109
argos::CDynamics2DVelocityControl::SetAngularVelocity
void SetAngularVelocity(Real f_velocity)
Definition: dynamics2d_velocity_control.cpp:116
argos::CDynamics2DVelocityControl::m_ptAngularConstraint
cpConstraint * m_ptAngularConstraint
Definition: dynamics2d_velocity_control.h:61
argos::CDynamics2DVelocityControl::m_cDyn2DEngine
CDynamics2DEngine & m_cDyn2DEngine
Definition: dynamics2d_velocity_control.h:53
argos::CDynamics2DVelocityControl::CDynamics2DVelocityControl
CDynamics2DVelocityControl(CDynamics2DEngine &c_engine, Real f_max_force, Real f_max_torque)
Definition: dynamics2d_velocity_control.cpp:15
argos::CDynamics2DVelocityControl::m_fMaxForce
Real m_fMaxForce
Definition: dynamics2d_velocity_control.h:63
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CDynamics2DVelocityControl::m_ptControlBody
cpBody * m_ptControlBody
Definition: dynamics2d_velocity_control.h:55
argos::CDynamics2DVelocityControl::AttachTo
void AttachTo(cpBody *pt_body)
Definition: dynamics2d_velocity_control.cpp:38