physx_epuck_model.cpp
Go to the documentation of this file.
1 
7 #include "physx_epuck_model.h"
8 #include <argos3/plugins/robots/e-puck/simulator/epuck_entity.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
15  static const Real EPUCK_INTERWHEEL_DISTANCE = 0.053f;
16  static const Real EPUCK_WHEEL_RADIUS = 0.0205f;
17  static const Real EPUCK_WHEEL_THICKNESS = 0.01f;
18  static const Real EPUCK_WHEEL_MASS = 0.05f;
19 
20  static const Real EPUCK_CHASSIS_LENGTH = 0.055f;
21  static const Real EPUCK_CHASSIS_WIDTH = (EPUCK_INTERWHEEL_DISTANCE - EPUCK_WHEEL_THICKNESS) * 0.9f;
22  static const Real EPUCK_CHASSIS_HEIGHT = EPUCK_WHEEL_RADIUS * 2.0f;
23  static const Real EPUCK_CHASSIS_ELEVATION = 0.005f;
24  static const Real EPUCK_CHASSIS_MASS = 0.3f;
25 
26  static const Real EPUCK_BOARD_RADIUS = 0.035f;
27  static const Real EPUCK_BOARD_HEIGHT = 0.03f;
28 
29  enum EPUCK_WHEELS {
30  EPUCK_LEFT_WHEEL = 0,
31  EPUCK_RIGHT_WHEEL = 1
32  };
33 
34  /****************************************/
35  /****************************************/
36 
37  CPhysXEPuckModel::CPhysXEPuckModel(CPhysXEngine& c_engine,
38  CEPuckEntity& c_entity) :
39  CPhysXMultiBodyObjectModel(c_engine, c_entity, 3),
40  m_fCurrentWheelVelocity(c_entity.GetWheeledEntity().GetWheelVelocities()),
41  m_cDiffDrive(*this,
42  c_engine,
43  EPUCK_INTERWHEEL_DISTANCE,
44  EPUCK_WHEEL_RADIUS,
45  EPUCK_WHEEL_THICKNESS,
46  EPUCK_WHEEL_MASS,
47  physx::PxVec3(EPUCK_CHASSIS_LENGTH,
48  EPUCK_CHASSIS_WIDTH,
49  EPUCK_CHASSIS_HEIGHT),
50  EPUCK_CHASSIS_ELEVATION,
51  EPUCK_CHASSIS_MASS) {
52  /* Get position and orientation in this engine's representation */
53  physx::PxTransform cBodyTrans;
54  CVector3ToPxVec3(GetEmbodiedEntity().GetOriginAnchor().Position, cBodyTrans.p);
55  CQuaternionToPxQuat(GetEmbodiedEntity().GetOriginAnchor().Orientation, cBodyTrans.q);
56  /*
57  * Top board
58  */
59  /* Create cylinder geometry */
60  physx::PxConvexMeshGeometry* pcBoardGeometry =
61  CreateCylinderGeometry(c_engine,
62  EPUCK_BOARD_RADIUS,
63  EPUCK_BOARD_HEIGHT);
64  /* Create the shape and attach it to the differential drive component */
65  physx::PxShape* pcBoardShape =
66  m_cDiffDrive.GetMainBodyActor().createShape(*pcBoardGeometry,
67  GetPhysXEngine().GetDefaultMaterial());
68  pcBoardShape->userData = this;
69  /* Set offset transformation for the board */
70  pcBoardShape->setLocalPose(
71  physx::PxTransform(0.0f,
72  0.0f,
73  EPUCK_CHASSIS_ELEVATION +
74  EPUCK_CHASSIS_HEIGHT +
75  EPUCK_BOARD_HEIGHT * 0.5f));
76  /* Place the differential drive component in its initial position */
77  m_cDiffDrive.SetGlobalPose(cBodyTrans);
78  /* Calculate the bounding box */
79  CalculateBoundingBox();
80  /* Cleanup */
81  delete pcBoardGeometry;
82  /* Register the origin anchor update method */
83  RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
84  &CPhysXEPuckModel::UpdateOriginAnchor);
85  }
86 
87  /****************************************/
88  /****************************************/
89 
90  void CPhysXEPuckModel::Reset() {
91  /* Reset bodies */
92  CPhysXMultiBodyObjectModel::Reset();
93  /* Zero wheel speeds */
94  m_cDiffDrive.SetTargetWheelLinearVelocity(0.0f, 0.0f);
95  }
96 
97  /****************************************/
98  /****************************************/
99 
100  void CPhysXEPuckModel::UpdateFromEntityStatus() {
101  /* Do we want to move? */
102  if((m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL] != 0.0f) ||
103  (m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL] != 0.0f)) {
104  m_cDiffDrive.SetTargetWheelLinearVelocity
105  (m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL],
106  m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL]);
107  }
108  else {
109  /* No, we don't want to move - zero all speeds */
110  m_cDiffDrive.SetTargetWheelLinearVelocity(0.0f, 0.0f);
111  }
112  }
113 
114  /****************************************/
115  /****************************************/
116 
117  void CPhysXEPuckModel::UpdateOriginAnchor(SAnchor& s_anchor) {
118  /* Get transform of the ARGoS origin anchor */
119  physx::PxTransform cBodyTrans =
120  m_cDiffDrive.GetMainBodyActor().getGlobalPose() *
121  m_cDiffDrive.GetMainBodyOffset().getInverse();
122  /* Set object position into ARGoS space */
123  PxVec3ToCVector3(cBodyTrans.p, s_anchor.Position);
124  /* Set object orientation into ARGoS space */
125  PxQuatToCQuaternion(cBodyTrans.q, s_anchor.Orientation);
126  physx::PxReal fLeftAngle;
127  physx::PxReal fRightAngle;
128  m_cDiffDrive.GetCurrentWheelAngles(fLeftAngle, fRightAngle);
129  DEBUG("Current wheel rotation: %f, %f\n", fLeftAngle, fRightAngle);
130  physx::PxReal fLeftVelocity;
131  physx::PxReal fRightVelocity;
132  m_cDiffDrive.GetCurrentWheelLinearVelocity(fLeftVelocity, fRightVelocity);
133  DEBUG("Current wheel speed: %f, %f\n", fLeftVelocity, fRightVelocity);
134  physx::PxReal fLeftTargetVelocity;
135  physx::PxReal fRightTargetVelocity;
136  m_cDiffDrive.GetTargetWheelLinearVelocity(fLeftTargetVelocity, fRightTargetVelocity);
137  DEBUG("Target wheel speed: %f, %f\n\n", fLeftTargetVelocity, fRightTargetVelocity);
138  }
139 
140  /****************************************/
141  /****************************************/
142 
143  REGISTER_STANDARD_PHYSX_OPERATIONS_ON_ENTITY(CEPuckEntity, CPhysXEPuckModel);
144 
145  /****************************************/
146  /****************************************/
147 
148 }
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
DEBUG
#define DEBUG(MSG,...)
Definition: argos_log.h:48
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::REGISTER_STANDARD_PHYSX_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_PHYSX_OPERATIONS_ON_ENTITY(CEPuckEntity, CPhysXEPuckModel)
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::CPhysXEPuckModel::CPhysXEPuckModel
CPhysXEPuckModel(CPhysXEngine &c_engine, CEPuckEntity &c_entity)
Definition: physx_epuck_model.cpp:37
argos::CPhysXEPuckModel::UpdateFromEntityStatus
virtual void UpdateFromEntityStatus()
Definition: physx_epuck_model.cpp:100
argos::CPhysXEPuckModel::UpdateOriginAnchor
void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
Definition: physx_epuck_model.cpp:117