9 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
10 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
18 static const Real PD_P_CONSTANT = 0.4;
19 static const Real PD_D_CONSTANT = 0.2;
21 static const Real FOOTBOT_RADIUS = 0.085036758f;
22 static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
23 static const Real FOOTBOT_HEIGHT = 0.146899733f;
25 static const Real FOOTBOT_MAX_FORCE = 15.f;
26 static const Real FOOTBOT_MAX_TORQUE = 150.f;
46 m_cFootBotEntity(c_entity),
47 m_cWheeledEntity(m_cFootBotEntity.GetWheeledEntity()),
48 m_cGripperEntity(c_entity.GetGripperEquippedEntity()),
49 m_cDiffSteering(c_engine,
52 FOOTBOT_INTERWHEEL_DISTANCE),
56 m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()),
57 m_unLastTurretMode(m_cFootBotEntity.GetTurretEntity().GetMode()) {
58 RegisterAnchorMethod<CDynamics2DFootBotModel>(
61 RegisterAnchorMethod<CDynamics2DFootBotModel>(
64 RegisterAnchorMethod<CDynamics2DFootBotModel>(
71 cpMomentForCircle(m_fMass,
73 FOOTBOT_RADIUS + FOOTBOT_RADIUS,
76 m_ptActualBaseBody->p = cpv(cPosition.
GetX(), cPosition.
GetY());
79 cpBodySetAngle(m_ptActualBaseBody, cZAngle.
GetValue());
83 cpCircleShapeNew(m_ptActualBaseBody,
86 m_ptBaseShape->e = 0.0;
87 m_ptBaseShape->u = 0.7;
92 m_cDiffSteering.
AttachTo(m_ptActualBaseBody);
94 AddBody(m_ptActualBaseBody, cpvzero, 0, FOOTBOT_HEIGHT);
96 m_ptActualGripperBody =
98 cpBodyNew(m_fMass / 20.0,
99 cpMomentForCircle(m_fMass,
101 FOOTBOT_RADIUS + FOOTBOT_RADIUS,
103 m_ptActualGripperBody->p = cpv(cPosition.
GetX(), cPosition.
GetY());
104 cpBodySetAngle(m_ptActualGripperBody,
108 cpShape* ptGripperShape =
110 cpCircleShapeNew(m_ptActualGripperBody,
112 cpv(FOOTBOT_RADIUS, 0.0f)));
117 m_ptBaseGripperLinearMotion =
119 cpPivotJointNew2(m_ptActualBaseBody,
120 m_ptActualGripperBody,
123 m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
124 cpGearJointNew(m_ptActualBaseBody,
125 m_ptActualGripperBody,
128 m_ptBaseGripperAngularMotion->maxBias = 0.0f;
129 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
131 AddBody(m_ptActualGripperBody, cpvzero, 0, FOOTBOT_HEIGHT);
135 TurretActiveToPassive();
144 delete m_pcGrippable;
145 switch(m_unLastTurretMode) {
148 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
149 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
150 cpConstraintFree(m_ptBaseGripperLinearMotion);
151 cpConstraintFree(m_ptBaseGripperAngularMotion);
155 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
156 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
157 cpConstraintFree(m_ptBaseGripperLinearMotion);
158 cpConstraintFree(m_ptGripperControlAngularMotion);
159 cpBodyFree(m_ptControlGripperBody);
183 m_cDiffSteering.
Reset();
190 TurretActiveToPassive();
222 m_cDiffSteering.
Reset();
234 switch(m_unLastTurretMode) {
240 TurretPassiveToActive();
252 TurretActiveToPassive();
264 switch(m_unLastTurretMode) {
272 m_ptControlGripperBody->w =
274 (PD_P_CONSTANT * fCurRotErr +
276 m_fPreviousTurretAngleError = fCurRotErr;
280 m_ptControlGripperBody->w =
288 m_ptBaseGripperAngularMotion->maxForce = 0.0001f;
291 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
300 void CDynamics2DFootBotModel::TurretPassiveToActive() {
302 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
303 cpConstraintFree(m_ptBaseGripperAngularMotion);
305 m_ptControlGripperBody = cpBodyNew(INFINITY, INFINITY);
307 m_ptGripperControlAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
308 cpGearJointNew(m_ptActualGripperBody,
309 m_ptControlGripperBody,
312 m_ptGripperControlAngularMotion->maxBias = 0.0f;
313 m_ptGripperControlAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
319 void CDynamics2DFootBotModel::TurretActiveToPassive() {
321 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
322 cpConstraintFree(m_ptGripperControlAngularMotion);
324 cpBodyFree(m_ptControlGripperBody);
326 m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
327 cpGearJointNew(m_ptActualBaseBody,
328 m_ptActualGripperBody,
331 m_ptBaseGripperAngularMotion->maxBias = 0.0f;
332 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;