dynamics3d_prototype_model.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/robots/prototype/simulator/prototype_entity.h>
9 #include <argos3/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.h>
10 #include <argos3/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h>
11 #include <argos3/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.h>
12 #include <argos3/plugins/simulator/physics_engines/dynamics3d/dynamics3d_shape_manager.h>
13 #include <argos3/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.h>
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  std::shared_ptr<btCollisionShape> CDynamics3DPrototypeModel::RequestShape(const CPrototypeLinkEntity& c_link_entity) {
21  btVector3 cHalfExtents(c_link_entity.GetExtents().GetX() * 0.5f,
22  c_link_entity.GetExtents().GetZ() * 0.5f,
23  c_link_entity.GetExtents().GetY() * 0.5f);
24  std::shared_ptr<btCollisionShape> pcShape;
25  switch(c_link_entity.GetGeometry()) {
26  case CPrototypeLinkEntity::EGeometry::BOX:
27  pcShape = CDynamics3DShapeManager::RequestBox(cHalfExtents);
28  break;
29  case CPrototypeLinkEntity::EGeometry::CYLINDER:
30  pcShape = CDynamics3DShapeManager::RequestCylinder(cHalfExtents);
31  break;
32  case CPrototypeLinkEntity::EGeometry::SPHERE:
33  pcShape = CDynamics3DShapeManager::RequestSphere(cHalfExtents.getZ());
34  break;
35  default:
36  THROW_ARGOSEXCEPTION("Collision shape geometry not implemented");
37  break;
38  }
39  return pcShape;
40  }
41 
42  /****************************************/
43  /****************************************/
44 
45  CDynamics3DMultiBodyObjectModel::CAbstractBody::SData
46  CDynamics3DPrototypeModel::CreateBodyData(const CPrototypeLinkEntity& c_link_entity) {
47  /* Get friction */
48  btScalar fFriction = GetEngine().GetDefaultFriction();
49  /* Calculate inertia */
50  btScalar fMass = c_link_entity.GetMass();
51  btVector3 cInertia;
52  RequestShape(c_link_entity)->calculateLocalInertia(fMass, cInertia);
53  /* Calculate start transform */
54  const CVector3& cPosition = c_link_entity.GetAnchor().Position;
55  const CQuaternion& cOrientation = c_link_entity.GetAnchor().Orientation;
56  btTransform cStartTransform(btQuaternion(cOrientation.GetX(),
57  cOrientation.GetZ(),
58  -cOrientation.GetY(),
59  cOrientation.GetW()),
60  btVector3(cPosition.GetX(),
61  cPosition.GetZ(),
62  -cPosition.GetY()));
63  /* Calculate center of mass offset */
64  btTransform cCenterOfMassOffset(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f),
65  btVector3(0.0f, -c_link_entity.GetExtents().GetZ() * 0.5f, 0.0f));
66  /* Return link data */
67  return CAbstractBody::SData(cStartTransform, cCenterOfMassOffset, cInertia, fMass, fFriction);
68  }
69 
70  /****************************************/
71  /****************************************/
72 
74  CPrototypeEntity& c_entity) :
76  c_entity,
77  c_entity.GetLinkEquippedEntity().GetLinks().size() - 1,
78  !c_entity.GetEmbodiedEntity().IsMovable()),
79  m_cEntity(c_entity),
80  m_cJointEquippedEntity(c_entity.GetJointEquippedEntity()) {
81  /* Use the reference link as the base of the robot */
83  /* Get the collision shape */
84  std::shared_ptr<btCollisionShape> ptrBaseLinkShape = RequestShape(cBaseLink);
85  /* Set up the base link body */
86  CBase* pcBase =
87  new CBase(*this,
88  cBaseLink.GetAnchor(),
89  ptrBaseLinkShape,
90  CreateBodyData(cBaseLink));
91  /* Add to collection */
92  m_vecBodies.push_back(pcBase);
93  /* Counter for enumerating the links inside the btMultiBody */
94  UInt32 unLinkIndex = 0;
95  /* Copy all joints pointers into a temporary vector */
96  CPrototypeJointEntity::TVector vecJointsToAdd =
97  m_cJointEquippedEntity.GetJoints();
98  /* While there are joints to be added */
99  while(! vecJointsToAdd.empty()) {
100  size_t unRemainingJoints = vecJointsToAdd.size();
101  for(CPrototypeJointEntity::TVectorIterator it_joint = std::begin(vecJointsToAdd);
102  it_joint != std::end(vecJointsToAdd);
103  ++it_joint) {
104  /* Get a reference to the joint */
105  CPrototypeJointEntity& cJoint = (**it_joint);
106  /* Get a reference to the parent link entity */
107  const CPrototypeLinkEntity& cParentLink = cJoint.GetParentLink();
108  /* Check if the parent link has been added */
109  CAbstractBody::TVectorIterator itParentLinkBody =
110  std::find_if(std::begin(m_vecBodies),
111  std::end(m_vecBodies),
112  [&cParentLink] (CAbstractBody* pc_body) {
113  return (cParentLink.GetAnchor().Index == pc_body->GetAnchor().Index);
114  });
115  /* If the parent link hasn't been added yet, try the next joint */
116  if(itParentLinkBody == std::end(m_vecBodies)) {
117  continue;
118  }
119  /* There should be no need to check the result of this cast */
120  CLink* pcParentLinkBody = dynamic_cast<CLink*>(*itParentLinkBody);
121  /* Get a reference to the child link */
122  CPrototypeLinkEntity& cChildLink = cJoint.GetChildLink();
123  /* Get the collision shape */
124  std::shared_ptr<btCollisionShape> ptrChildLinkShape =
125  RequestShape(cChildLink);
126  /* Set up the child link body */
127  CLink* pcChildLinkBody =
128  new CLink(*this,
129  unLinkIndex++,
130  cChildLink.GetAnchor(),
131  ptrChildLinkShape,
132  CreateBodyData(cChildLink));
133  /* Add to collection */
134  m_vecBodies.push_back(pcChildLinkBody);
135  /* Calculate joint parameters for parent link */
136  const CVector3& cParentOffsetPosition = cJoint.GetParentLinkJointPosition();
137  const CQuaternion& cParentOffsetOrientation = cJoint.GetParentLinkJointOrientation();
138  btTransform cParentOffsetTransform =
139  btTransform(btQuaternion(cParentOffsetOrientation.GetX(),
140  cParentOffsetOrientation.GetZ(),
141  -cParentOffsetOrientation.GetY(),
142  cParentOffsetOrientation.GetW()),
143  btVector3(cParentOffsetPosition.GetX(),
144  cParentOffsetPosition.GetZ(),
145  -cParentOffsetPosition.GetY()));
146  cParentOffsetTransform = pcParentLinkBody->GetData().CenterOfMassOffset * cParentOffsetTransform;
147  /* Calculate joint parameters for child link */
148  const CVector3& cChildOffsetPosition = cJoint.GetChildLinkJointPosition();
149  const CQuaternion& cChildOffsetOrientation = cJoint.GetChildLinkJointOrientation();
150  btTransform cChildOffsetTransform =
151  btTransform(btQuaternion(cChildOffsetOrientation.GetX(),
152  cChildOffsetOrientation.GetZ(),
153  -cChildOffsetOrientation.GetY(),
154  cChildOffsetOrientation.GetW()),
155  btVector3(cChildOffsetPosition.GetX(),
156  cChildOffsetPosition.GetZ(),
157  -cChildOffsetPosition.GetY()));
158  cChildOffsetTransform = pcChildLinkBody->GetData().CenterOfMassOffset * cChildOffsetTransform;
159  /* Calculate the joint axis */
160  btVector3 cJointAxis(cJoint.GetJointAxis().GetX(),
161  cJoint.GetJointAxis().GetZ(),
162  -cJoint.GetJointAxis().GetY());
163  /* Calculate the parent to child joint rotation */
164  /*
165  // From testing, these both the of following solutions seem correct although one may be wrong
166  btQuaternion cParentToChildRotation = cParentOffsetTransform.inverse().getRotation() *
167  cChildOffsetTransform.getRotation();
168  */
169  btQuaternion cParentToChildRotation = cChildOffsetTransform.getRotation() *
170  cParentOffsetTransform.inverse().getRotation();
171  /* Store joint configuration for reset */
172  m_vecJoints.emplace_back(cJoint.GetType(),
173  *pcParentLinkBody,
174  *pcChildLinkBody,
175  cParentOffsetTransform.getOrigin(),
176  -cChildOffsetTransform.getOrigin(),
177  cParentToChildRotation,
178  cJointAxis,
179  cJoint.GetDisableCollision());
180  /* Sensor and actuator configuration for revolute and prismatic joints */
183  /* If the joint actuator isn't disabled */
184  CPrototypeJointEntity::SActuator& sActuator = cJoint.GetActuator();
186  m_vecActuators.emplace_back(*this, sActuator, pcChildLinkBody->GetIndex());
187  }
188  /* And if the joint sensor isn't disabled */
189  CPrototypeJointEntity::SSensor& sSensor = cJoint.GetSensor();
191  m_vecSensors.emplace_back(*this, sSensor, pcChildLinkBody->GetIndex());
192  }
193  }
194  /* Joint limits */
195  if(cJoint.HasLimit()) {
197  const CRange<CRadians>& cLimit = cJoint.GetLimit().Revolute;
198  m_vecLimits.emplace_back(*this,
199  cLimit.GetMin().GetValue(),
200  cLimit.GetMax().GetValue(),
201  pcChildLinkBody->GetIndex());
202  }
203  else if(cJoint.GetType() == CPrototypeJointEntity::EType::PRISMATIC) {
204  const CRange<Real>& cLimit = cJoint.GetLimit().Prismatic;
205  m_vecLimits.emplace_back(*this,
206  cLimit.GetMin(),
207  cLimit.GetMax(),
208  pcChildLinkBody->GetIndex());
209  }
210  }
211  /* Now that the joint and link has been added we remove it from the vector
212  and restart the loop */
213  vecJointsToAdd.erase(it_joint);
214  break;
215  }
216  if(unRemainingJoints == vecJointsToAdd.size()) {
217  CPrototypeJointEntity* pcJoint = vecJointsToAdd.front();
218  THROW_ARGOSEXCEPTION("Can not add link \"" <<
219  pcJoint->GetChildLink().GetId() <<
220  "\" to the model before its parent link \"" <<
221  pcJoint->GetParentLink().GetId() <<
222  "\" has been added.");
223  }
224  }
225  for(CPrototypeLinkEntity* pc_link : m_cEntity.GetLinkEquippedEntity().GetLinks()) {
226  if(std::find_if(std::begin(m_vecBodies),
227  std::end(m_vecBodies),
228  [pc_link] (CAbstractBody* pc_body) {
229  return (pc_link->GetAnchor().Index == pc_body->GetAnchor().Index);
230  }) == std::end(m_vecBodies)) {
231  THROW_ARGOSEXCEPTION("Link \"" << pc_link->GetId() <<
232  "\" is not connected to the model.");
233 
234  }
235  }
236  /* With this call to reset, the multi-body model is configured and ready to be added */
237  Reset();
238  }
239 
240  /****************************************/
241  /****************************************/
242 
243  void CDynamics3DPrototypeModel::AddToWorld(btMultiBodyDynamicsWorld& c_world) {
244  /* Call CDynamics3DMultiBodyObjectModel::AddToWorld to add all bodies to the world */
246  /* Add the actuators (btMultiBodyJointMotors) constraints to the world */
247  for(SActuator& s_actuator : m_vecActuators) {
248  c_world.addMultiBodyConstraint(&s_actuator.Motor);
249  }
250  /* Add the joint limits to the world */
251  for(SLimit& s_limit : m_vecLimits) {
252  c_world.addMultiBodyConstraint(&s_limit.Constraint);
253  }
254  }
255 
256  /****************************************/
257  /****************************************/
258 
259  void CDynamics3DPrototypeModel::RemoveFromWorld(btMultiBodyDynamicsWorld& c_world) {
260  /* Remove the joint limits to the world */
261  for(SLimit& s_limit : m_vecLimits) {
262  c_world.removeMultiBodyConstraint(&s_limit.Constraint);
263  }
264  /* Remove the actuators (btMultiBodyJointMotors) constraints to the world */
265  for(SActuator& s_actuator : m_vecActuators) {
266  c_world.removeMultiBodyConstraint(&s_actuator.Motor);
267  }
268  /* Call CDynamics3DMultiBodyObjectModel::RemoveFromWorld to remove all bodies to the world */
270  }
271 
272  /****************************************/
273  /****************************************/
274 
276  /* Reset the base class (recreates the btMultiBody and calls reset on the bodies) */
278  /* Add setup the links and joints */
279  for(SJoint& s_joint : m_vecJoints) {
280  switch(s_joint.Type) {
282  m_cMultiBody.setupFixed(s_joint.Child.GetIndex(),
283  s_joint.Child.GetData().Mass,
284  s_joint.Child.GetData().Inertia,
285  s_joint.Parent.GetIndex(),
286  s_joint.ParentToChildRotation,
287  s_joint.ParentOffset,
288  s_joint.ChildOffset);
289  break;
291  m_cMultiBody.setupSpherical(s_joint.Child.GetIndex(),
292  s_joint.Child.GetData().Mass,
293  s_joint.Child.GetData().Inertia,
294  s_joint.Parent.GetIndex(),
295  s_joint.ParentToChildRotation,
296  s_joint.ParentOffset,
297  s_joint.ChildOffset,
298  s_joint.DisableCollision);
299  break;
301  m_cMultiBody.setupRevolute(s_joint.Child.GetIndex(),
302  s_joint.Child.GetData().Mass,
303  s_joint.Child.GetData().Inertia,
304  s_joint.Parent.GetIndex(),
305  s_joint.ParentToChildRotation,
306  s_joint.Axis,
307  s_joint.ParentOffset,
308  s_joint.ChildOffset,
309  s_joint.DisableCollision);
310  break;
312  m_cMultiBody.setupPrismatic(s_joint.Child.GetIndex(),
313  s_joint.Child.GetData().Mass,
314  s_joint.Child.GetData().Inertia,
315  s_joint.Parent.GetIndex(),
316  s_joint.ParentToChildRotation,
317  s_joint.Axis,
318  s_joint.ParentOffset,
319  s_joint.ChildOffset,
320  s_joint.DisableCollision);
321  break;
322  default:
323  break;
324  }
325  }
326  /* Reset the actuators */
327  for(SActuator& s_actuator : m_vecActuators) {
328  s_actuator.Reset();
329  }
330  /* Reset the joint limits */
331  for(SLimit& s_limit : m_vecLimits) {
332  s_limit.Reset();
333  }
334  /* Allocate memory and prepare the btMultiBody */
335  m_cMultiBody.finalizeMultiDof();
336  /* Synchronize with the entity in the space */
338  }
339 
340  /****************************************/
341  /****************************************/
342 
344  /* Write back sensor data to the joints */
345  for(SSensor& s_sensor : m_vecSensors) {
346  s_sensor.Update();
347  }
348  /* Update anchors using the base class method */
350  }
351 
352  /****************************************/
353  /****************************************/
354 
356  /* Read in actuator data from the joints */
357  for(SActuator& s_actuator : m_vecActuators) {
358  s_actuator.Update();
359  }
360  }
361 
362  /****************************************/
363  /****************************************/
364 
365  CDynamics3DPrototypeModel::SJoint::SJoint(CPrototypeJointEntity::EType e_type,
366  CLink& c_parent,
367  CLink& c_child,
368  const btVector3& c_parent_offset,
369  const btVector3& c_child_offset,
370  const btQuaternion& c_parent_to_child_rotation,
371  const btVector3& c_axis,
372  bool b_disable_collision) :
373  Type(e_type),
374  Parent(c_parent),
375  Child(c_child),
376  ParentOffset(c_parent_offset),
377  ChildOffset(c_child_offset),
378  ParentToChildRotation(c_parent_to_child_rotation),
379  Axis(c_axis),
380  DisableCollision(b_disable_collision) {}
381 
382  /****************************************/
383  /****************************************/
384 
385  CDynamics3DPrototypeModel::SLimit::SLimit(CDynamics3DPrototypeModel& c_model,
386  Real f_lower_limit,
387  Real f_upper_limit,
388  SInt32 n_joint_index) :
389  Model(c_model),
390  LowerLimit(f_lower_limit),
391  UpperLimit(f_upper_limit),
392  JointIndex(n_joint_index),
393  Constraint(&c_model.GetMultiBody(),
394  n_joint_index,
395  f_lower_limit,
396  f_upper_limit) {}
397 
398  /****************************************/
399  /****************************************/
400 
401  void CDynamics3DPrototypeModel::SLimit::Reset() {
402  Constraint.~btMultiBodyJointLimitConstraint();
403  new (&Constraint) btMultiBodyJointLimitConstraint(&Model.GetMultiBody(),
404  JointIndex,
405  LowerLimit,
406  UpperLimit);
407  }
408 
409  /****************************************/
410  /****************************************/
411 
412  CDynamics3DPrototypeModel::SActuator::SActuator(CDynamics3DPrototypeModel& c_model,
413  CPrototypeJointEntity::SActuator& s_actuator,
414  SInt32 n_joint_index) :
415  Model(c_model),
416  Actuator(s_actuator),
417  JointIndex(n_joint_index),
418  Motor(&c_model.GetMultiBody(),
419  n_joint_index,
420  0,
421  s_actuator.Target,
422  s_actuator.MaxImpulse) {}
423 
424  /****************************************/
425  /****************************************/
426 
427  void CDynamics3DPrototypeModel::SActuator::Reset() {
428  Motor.~btMultiBodyJointMotor();
429  new (&Motor) btMultiBodyJointMotor(&Model.GetMultiBody(),
430  JointIndex,
431  0,
432  Actuator.Target,
433  Actuator.MaxImpulse);
434  }
435 
436  /****************************************/
437  /****************************************/
438 
439  void CDynamics3DPrototypeModel::SActuator::Update() {
441  Motor.setPositionTarget(Actuator.Target);
442  }
443  else if(Actuator.Mode == CPrototypeJointEntity::SActuator::EMode::VELOCITY) {
444  Motor.setVelocityTarget(Actuator.Target);
445  }
446  }
447 
448  /****************************************/
449  /****************************************/
450 
451  CDynamics3DPrototypeModel::SSensor::SSensor(CDynamics3DPrototypeModel& c_model,
452  CPrototypeJointEntity::SSensor& s_sensor,
453  SInt32 n_joint_index) :
454  Model(c_model),
455  Sensor(s_sensor),
456  JointIndex(n_joint_index) {}
457 
458  /****************************************/
459  /****************************************/
460 
461  void CDynamics3DPrototypeModel::SSensor::Update() {
463  Sensor.Value = Model.GetMultiBody().getJointPos(JointIndex);
464  }
465  else if(Sensor.Mode == CPrototypeJointEntity::SSensor::EMode::VELOCITY) {
466  Sensor.Value = Model.GetMultiBody().getJointVel(JointIndex);
467  }
468  }
469 
470  /****************************************/
471  /****************************************/
472 
473  REGISTER_STANDARD_DYNAMICS3D_OPERATIONS_ON_ENTITY(CPrototypeEntity, CDynamics3DPrototypeModel);
474 
475  /****************************************/
476  /****************************************/
477 
478 }
479 
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
static std::shared_ptr< btCollisionShape > RequestSphere(btScalar f_radius)
virtual void AddToWorld(btMultiBodyDynamicsWorld &c_world)
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
A 3D vector class.
Definition: vector3.h:29
const CQuaternion & GetParentLinkJointOrientation() const
const CQuaternion & GetChildLinkJointOrientation() const
virtual void RemoveFromWorld(btMultiBodyDynamicsWorld &c_world)
Real GetX() const
Definition: quaternion.h:53
CDynamics3DEngine & GetEngine()
T GetMax() const
Definition: range.h:48
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
Real GetZ() const
Definition: quaternion.h:61
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
CPrototypeLinkEntity::TVector & GetLinks()
virtual void AddToWorld(btMultiBodyDynamicsWorld &c_world)
const CVector3 & GetParentLinkJointPosition() const
std::vector< CPrototypeJointEntity * > TVector
UInt32 Index
The index of the anchor assigned by the embodied entity.
Definition: physics_model.h:45
std::vector< CAbstractBody * > m_vecBodies
static std::shared_ptr< btCollisionShape > RequestCylinder(const btVector3 &c_half_extents)
CPrototypeLinkEntity & GetParentLink()
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
T GetMin() const
Definition: range.h:32
std::vector< CAbstractBody * >::iterator TVectorIterator
CPrototypeLinkEntity & GetChildLink()
virtual void RemoveFromWorld(btMultiBodyDynamicsWorld &c_world)
const CVector3 & GetChildLinkJointPosition() const
btScalar GetDefaultFriction() const
Real GetW() const
Definition: quaternion.h:49
CPrototypeLinkEquippedEntity & GetLinkEquippedEntity()
std::vector< CPrototypeJointEntity * >::iterator TVectorIterator
const CVector3 & GetJointAxis() const
REGISTER_STANDARD_DYNAMICS3D_OPERATIONS_ON_ENTITY(CPrototypeEntity, CDynamics3DPrototypeModel)
Real GetY() const
Definition: quaternion.h:57
CPrototypeJointEntity::TVector & GetJoints()
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
CDynamics3DPrototypeModel(CDynamics3DEngine &c_engine, CPrototypeEntity &c_entity)
const ULimit & GetLimit() const
static std::shared_ptr< btCollisionShape > RequestBox(const btVector3 &c_half_extents)