Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Contacts.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/Contacts.h"
3
4#include <rbdl/Kinematics.h>
5
6#include "BiorbdModel.h"
7#include "RigidBody/ExternalForceSet.h"
8#include "RigidBody/GeneralizedAcceleration.h"
9#include "RigidBody/GeneralizedCoordinates.h"
10#include "RigidBody/GeneralizedTorque.h"
11#include "RigidBody/GeneralizedVelocity.h"
12#include "RigidBody/Joints.h"
13#include "RigidBody/NodeSegment.h"
14#include "RigidBody/Segment.h"
15#include "Utils/Error.h"
16#include "Utils/Matrix.h"
17#include "Utils/Rotation.h"
18#include "Utils/RotoTrans.h"
19#include "Utils/SpatialVector.h"
20#include "Utils/String.h"
21#include "Utils/Vector3d.h"
22
23using namespace BIORBD_NAMESPACE;
24
26 : RigidBodyDynamics::ConstraintSet(),
27 m_nbreConstraint(std::make_shared<size_t>(0)),
28 m_isBinded(std::make_shared<bool>(false)),
29 m_rigidContacts(std::make_shared<std::vector<rigidbody::NodeSegment>>()),
30 m_nbLoopConstraint(std::make_shared<size_t>(0)) {}
31
34 copy.DeepCopy(*this);
35 return copy;
36}
37
39 static_cast<RigidBodyDynamics::ConstraintSet&>(*this) = other;
42 *m_isBinded = *other.m_isBinded;
44}
45
47 size_t bodyId,
48 const utils::Vector3d& bodyPoint,
49 const utils::Vector3d& worldNormal,
50 const utils::String& name,
51 const utils::String& parentName) {
52#ifndef BIORBD_USE_CASADI_MATH
53#ifndef SKIP_ASSERT
55 worldNormal.norm() == 1.0, "Normal of the contact must be provided");
56
57#endif // !SKIP_ASSERT
58#endif // !BIORBD_USE_CASADI_MATH
59
61
62 // Check world_normal points to what axis
63 utils::String axis = "";
64 SCALAR_TO_DOUBLE(has_x, worldNormal[0]);
65 SCALAR_TO_DOUBLE(has_y, worldNormal[1]);
66 SCALAR_TO_DOUBLE(has_z, worldNormal[2]);
67 if (has_x != 0) {
68 axis += "x";
69 }
70 if (has_y != 0) {
71 axis += "y";
72 }
73 if (has_z != 0) {
74 axis += "z";
75 }
76
78 bodyPoint,
79 name,
80 parentName,
81 true,
82 false,
83 swapAxes(axis),
84 static_cast<int>(bodyId)));
85 return static_cast<size_t>(
86 RigidBodyDynamics::ConstraintSet::AddContactConstraint(
87 static_cast<unsigned int>(bodyId),
88 bodyPoint,
89 worldNormal,
90 name.c_str()));
91}
92
94 size_t bodyId,
95 const utils::Vector3d& bodyPoint,
96 const utils::String& axis,
97 const utils::String& name,
98 const utils::String& parentName) {
99 size_t ret(0);
100 for (size_t i = 0; i < axis.length(); ++i) {
102 if (axis.tolower()[i] == 'x') {
103 ret += static_cast<size_t>(
104 RigidBodyDynamics::ConstraintSet::AddContactConstraint(
105 static_cast<unsigned int>(bodyId),
106 bodyPoint,
107 utils::Vector3d(1, 0, 0),
108 (name + "_X").c_str()));
109 } else if (axis.tolower()[i] == 'y') {
110 ret += static_cast<size_t>(
111 RigidBodyDynamics::ConstraintSet::AddContactConstraint(
112 static_cast<unsigned int>(bodyId),
113 bodyPoint,
114 utils::Vector3d(0, 1, 0),
115 (name + "_Y").c_str()));
116 } else if (axis.tolower()[i] == 'z') {
117 ret += static_cast<size_t>(
118 RigidBodyDynamics::ConstraintSet::AddContactConstraint(
119 static_cast<unsigned int>(bodyId),
120 bodyPoint,
121 utils::Vector3d(0, 0, 1),
122 (name + "_Z").c_str()));
123 } else {
124 utils::Error::raise("Wrong axis!");
125 }
126 }
127
128 m_rigidContacts->push_back(NodeSegment(
129 bodyPoint,
130 name,
131 parentName,
132 true,
133 false,
134 swapAxes(axis),
135 static_cast<int>(bodyId)));
136 return ret;
137}
138
140 size_t body_id_predecessor,
141 size_t body_id_successor,
142 const utils::RotoTrans& X_predecessor,
143 const utils::RotoTrans& X_successor,
144 const utils::SpatialVector& axis,
145 const utils::String& name,
146 bool enableStabilization,
147 double stabilizationParam) {
150 return RigidBodyDynamics::ConstraintSet::AddLoopConstraint(
151 static_cast<unsigned int>(body_id_predecessor),
152 static_cast<unsigned int>(body_id_successor),
153 RigidBodyDynamics::Math::SpatialTransform(
154 X_predecessor.rot(), X_predecessor.trans()),
155 RigidBodyDynamics::Math::SpatialTransform(
156 X_successor.rot(), X_successor.trans()),
158 enableStabilization,
159 stabilizationParam,
160 name.c_str());
161}
162
163std::vector<utils::SpatialVector> rigidbody::Contacts::calcLoopConstraintForces(
166 const rigidbody::GeneralizedTorque& Tau) {
168 static_cast<BIORBD_NAMESPACE::Model&>(*this));
169 return calcLoopConstraintForces(Q, Qdot, Tau, forceSet);
170}
171std::vector<utils::SpatialVector> rigidbody::Contacts::calcLoopConstraintForces(
175 rigidbody::ExternalForceSet& externalForces) {
176 // all in the world frame
177 bool resolveAllInRootFrame = true;
178
179 // outputs
180 std::vector<unsigned int> constraintBodyIdsOutput;
181 std::vector<RigidBodyDynamics::Math::SpatialTransform>
182 updatedConstraintBodyFramesOutput;
183 std::vector<RigidBodyDynamics::Math::SpatialVector>
184 updatedConstraintForcesOutput;
185
186 // retrieve the model and the contacts
188 dynamic_cast<rigidbody::Contacts*>(this)->getConstraints();
189
190#ifdef BIORBD_USE_CASADI_MATH
192#else
194#endif
195 updatedModel =
196 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
197 &Q, &Qdot);
199 Q, Qdot, Tau, CS, externalForces, false);
200
201 std::vector<utils::SpatialVector> out;
202 for (int i = 0; i < static_cast<int>(*m_nbLoopConstraint); i++) {
203 constraintBodyIdsOutput.clear();
204 updatedConstraintBodyFramesOutput.clear();
205 updatedConstraintForcesOutput.clear();
206
207 CS.calcForces(
208 i,
209 updatedModel,
210 Q,
211 Qdot,
212 constraintBodyIdsOutput,
213 updatedConstraintBodyFramesOutput,
214 updatedConstraintForcesOutput,
215 resolveAllInRootFrame,
216 false);
217
218 // save all the forces in the global reference frame applied on the
219 // predecessor segment
220 out.push_back(updatedConstraintForcesOutput[0]);
221 }
222 return out;
223}
224
226
228 if (!*m_isBinded) {
229 // Assuming that this is also a Joints type (via BiorbdModel)
230 const rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
231 Bind(model);
232 *m_isBinded = true;
233 }
234 return *this;
235}
236
238 if (*m_nbreConstraint > 0)
239 return true;
240 else {
241 return false;
242 }
243}
244
246 if (*m_nbLoopConstraint > 0)
247 return true;
248 else {
249 return false;
250 }
251}
252
254
258
259std::vector<utils::String> rigidbody::Contacts::contactNames() {
260 std::vector<utils::String> names;
261 for (auto name : RigidBodyDynamics::ConstraintSet::name) {
262 names.push_back(name);
263 }
264 return names;
265}
266
269 i < *m_nbreConstraint, "Idx for contact names is too high..");
270 return RigidBodyDynamics::ConstraintSet::name[i];
271}
272
274 const utils::String& name) {
275 for (size_t i = 0; i < nbContacts(); ++i) {
276 if (contactName(i) == name) {
277 return i;
278 }
279 }
280
281 throw std::runtime_error("Contact '" + name + "' not found.");
282}
283
284std::vector<utils::Vector3d> rigidbody::Contacts::constraintsInGlobal(
286 bool updateKin) {
287 // Assuming that this is also a Joints type (via BiorbdModel)
288 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
289
290 // On each control, apply the rotation and save the position
291 std::vector<utils::Vector3d> tp;
292 for (size_t i = 0; i < contactConstraints.size(); ++i) {
293 for (size_t j = 0; j < contactConstraints[i]->getConstraintSize(); ++j) {
294 tp.push_back(model.CalcBodyToBaseCoordinates(
295 Q,
296 contactConstraints[i]->getBodyIds()[0],
297 contactConstraints[i]->getBodyFrames()[0].r,
298 updateKin));
299 updateKin = false;
300 }
301 }
302 return tp;
303}
304
307 size_t idx,
308 bool updateKin) {
309 // Assuming that this is also a joint type (via BiorbdModel)
310 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
311
312 // Calculate the acceleration of the contact
313 const rigidbody::NodeSegment& c = rigidContact(idx);
314 return model.CalcBodyToBaseCoordinates(Q, c.parentId(), c, updateKin);
315}
316
317std::vector<utils::Vector3d> rigidbody::Contacts::rigidContacts(
319 bool updateKin) {
320 // Assuming that this is also a Joints type (via BiorbdModel)
321 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
322
323 // Output variable
324 std::vector<utils::Vector3d> tp;
325
326 // On each control, apply the rotation and save the position
327 for (const rigidbody::NodeSegment& c : *m_rigidContacts) {
328 tp.push_back(
329 model.CalcBodyToBaseCoordinates(Q, c.parentId(), c, updateKin));
330 updateKin = false;
331 }
332
333 return tp;
334}
335
339 size_t idx,
340 bool updateKin) {
341 // Assuming that this is also a joint type (via BiorbdModel)
342 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
343
344 // Calculate the acceleration of the contact
345 const rigidbody::NodeSegment& c = rigidContact(idx);
346 return model.CalcPointVelocity(Q, Qdot, c.parentId(), c, updateKin);
347}
348
352 bool updateKin) {
353 // Assuming that this is also a joint type (via BiorbdModel)
354 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
355
356 // On each control, apply the Q, Qdot, Qddot and save the acceleration
357 std::vector<utils::Vector3d> tp;
358 for (const rigidbody::NodeSegment& c : *m_rigidContacts) {
359 tp.push_back(model.CalcPointVelocity(Q, Qdot, c.parentId(), c, updateKin));
360 updateKin = false;
361 }
362
363 return tp;
364}
365
370 size_t idx,
371 bool updateKin) {
372 // Assuming that this is also a joint type (via BiorbdModel)
373 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
374
375 // Calculate the acceleration of the contact
376 const rigidbody::NodeSegment& c = rigidContact(idx);
377 return model.CalcPointAcceleration(
378 Q, Qdot, Qddot, c.parentId(), c, updateKin);
379}
380
385 bool updateKin) {
386 // Assuming that this is also a joint type (via BiorbdModel)
387 rigidbody::Joints& model = dynamic_cast<rigidbody::Joints&>(*this);
388
389 // On each control, apply the Q, Qdot, Qddot and save the acceleration
390 std::vector<utils::Vector3d> tp;
391 for (const rigidbody::NodeSegment& c : *m_rigidContacts) {
392 tp.push_back(model.CalcPointAcceleration(
393 Q, Qdot, Qddot, c.parentId(), c, updateKin));
394 updateKin = false;
395 }
396
397 return tp;
398}
399
401 const utils::Matrix& J,
402 const NodeSegment& contact) {
403 const auto axes = contact.axes();
404
405 size_t nbActive = 0;
406 for (bool a : axes)
407 if (a) nbActive++;
408
409 utils::Matrix Jout(nbActive, J.cols());
410
411 size_t r = 0;
412 for (size_t i = 0; i < 3; ++i) {
413 if (axes[i]) {
414 Jout.block(r++, 0, 1, J.cols()) = J.block(i, 0, 1, J.cols());
415 }
416 }
417
418 return Jout;
419}
420
421// Get the Jacobian of the technical markers
423 rigidbody::Joints& updatedModel,
425 size_t idx) {
426 const auto& contact = rigidContact(idx);
427
428 utils::Matrix J =
429 updatedModel.CalcPointJacobian(Q, contact.parent(), contact, false);
430
431 return projectContactJacobian(J, contact);
432}
433
436 size_t idx,
437 bool updateKin) {
438#ifdef BIORBD_USE_CASADI_MATH
440#else
442#endif
443 updatedModel =
444 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
445 updateKin ? &Q : nullptr);
446
447 return rigidContactJacobian(updatedModel, Q, idx);
448}
449
452 bool updateKin) {
453#ifdef BIORBD_USE_CASADI_MATH
455#else
457#endif
458 updatedModel =
459 dynamic_cast<rigidbody::Joints&>(*this).UpdateKinematicsCustom(
460 updateKin ? &Q : nullptr);
461
462 return rigidContactsJacobian(updatedModel, Q);
463}
464
466 rigidbody::Joints& updatedModel,
468 std::vector<utils::Matrix> G;
469 G.reserve(nbRigidContacts());
470
471 for (size_t idx = 0; idx < nbRigidContacts(); ++idx) {
472 G.push_back(rigidContactJacobian(updatedModel, Q, idx));
473 }
474
475 return G;
476}
477
479 return static_cast<utils::Vector>(this->force);
480}
481
482const std::vector<rigidbody::NodeSegment>& rigidbody::Contacts::rigidContacts()
483 const {
484 return *m_rigidContacts;
485}
486
488 size_t idx) const {
489 // return (*m_rigidContacts)[idx];
490 return m_rigidContacts->at(idx);
491}
492
494 return static_cast<int>(m_rigidContacts->size());
495}
496
499 idx < nbRigidContacts(),
500 "Idx for rigid contact Segment Id is too high..");
501
502 // Assuming that this is also a joint type (via BiorbdModel)
503 const rigidbody::NodeSegment& c = rigidContact(idx);
504
505 const rigidbody::Joints& model =
506 dynamic_cast<const rigidbody::Joints&>(*this);
507 return model.getBodyRbdlIdToBiorbdId(c.parentId());
508}
509
511 int segment_idx) const {
512 // Output variable
513 std::vector<size_t> indices;
514
515 // On each rigidcontact, verify if it belongs to the segment specified
516 for (int i = 0; i < nbRigidContacts(); ++i) {
517 if (contactSegmentBiorbdId(i) == segment_idx) {
518 indices.push_back(i);
519 }
520 }
521
522 return indices;
523}
524
526 const utils::String& axesToSwap) const {
527 utils::String out;
528 std::string reference = "xyz";
529 std::set_difference(
530 reference.begin(),
531 reference.end(),
532 axesToSwap.begin(),
533 axesToSwap.end(),
534 std::back_inserter(out));
535 return out;
536}
The actual musculoskeletal model that holds everything in biorbd.
const std::vector< NodeSegment > & rigidContacts() const
Returns all the rigid contacts as declared in the model.
Definition Contacts.cpp:482
utils::Vector3d rigidContactVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the velocity of the chosen contact.
Definition Contacts.cpp:336
std::shared_ptr< std::vector< rigidbody::NodeSegment > > m_rigidContacts
Definition Contacts.h:393
std::vector< utils::String > contactNames()
Return the name of the all contacts.
Definition Contacts.cpp:259
std::shared_ptr< size_t > m_nbreConstraint
Number of constraints.
Definition Contacts.h:390
utils::Matrix rigidContactJacobian(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, size_t idx)
Compute the Jacobian of a single rigid contact.
Definition Contacts.cpp:422
size_t nbLoopConstraints() const
Return the number of loop constraints.
Definition Contacts.cpp:255
int contactSegmentBiorbdId(int idx) const
Return the segment idx of the contact in biorbd formalism.
Definition Contacts.cpp:497
static utils::Matrix projectContactJacobian(const utils::Matrix &J, const rigidbody::NodeSegment &contact)
Project a 3D point Jacobian onto active contact axes.
Definition Contacts.cpp:400
std::shared_ptr< size_t > m_nbLoopConstraint
Number of constraints.
Definition Contacts.h:395
size_t contactRbdlId(const utils::String &name)
Return the index of the contact corresponding to the given name.
Definition Contacts.cpp:273
utils::Vector getForce() const
Return the force acting on the constraint.
Definition Contacts.cpp:478
bool hasContacts() const
Check if there are contacts.
Definition Contacts.cpp:237
Contacts DeepCopy() const
Deep copy of contacts.
Definition Contacts.cpp:32
Contacts & getConstraints()
Get constraints.
Definition Contacts.cpp:227
bool hasLoopConstraints() const
Check if there are loop constraints.
Definition Contacts.cpp:245
utils::Vector3d rigidContactAcceleration(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &dQdot, size_t idx, bool updateKin=true)
Return the acceleration of the chosen contact.
Definition Contacts.cpp:366
std::vector< utils::Vector3d > rigidContactsAcceleration(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &dQdot, bool updateKin=true)
Return the acceleration of all the contacts.
Definition Contacts.cpp:381
size_t AddLoopConstraint(size_t body_id_predecessor, size_t body_id_successor, const utils::RotoTrans &X_predecessor, const utils::RotoTrans &X_successor, const utils::SpatialVector &axis, const utils::String &name, bool enableStabilization=false, double stabilizationParam=0.1)
Add a loop constraint to the constraint set.
Definition Contacts.cpp:139
size_t nbContacts() const
Return the number of contacts.
Definition Contacts.cpp:253
std::vector< utils::Vector3d > rigidContactsVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocities of all the contacts.
Definition Contacts.cpp:349
utils::String swapAxes(const utils::String &axesToSwap) const
Swap the status of the axes provided, that is adding x, y or z if it is not there and remove them oth...
Definition Contacts.cpp:525
virtual ~Contacts()
Destroy the class properly.
Definition Contacts.cpp:225
size_t AddConstraint(size_t bodyId, const utils::Vector3d &bodyPoint, const utils::Vector3d &worldNormal, const utils::String &name, const utils::String &parentName)
Add a constraint to the constraint set.
Definition Contacts.cpp:46
const NodeSegment & rigidContact(size_t idx) const
Returns the rigid contact idx as declared in the model.
Definition Contacts.cpp:487
std::vector< size_t > segmentRigidContactIdx(int segment_idx) const
Return the index of rigid contacts for a specified segment index.
Definition Contacts.cpp:510
std::vector< utils::Matrix > rigidContactsJacobian(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Compute the Jacobians of all rigid contacts.
Definition Contacts.cpp:465
std::shared_ptr< bool > m_isBinded
If the model is ready.
Definition Contacts.h:391
utils::String contactName(size_t i)
Return the name of the contact of a specified axis.
Definition Contacts.cpp:267
std::vector< utils::SpatialVector > calcLoopConstraintForces(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedTorque &Tau)
compute the forces generated from loop constraints
Definition Contacts.cpp:163
int nbRigidContacts() const
Returns the number of rigid contacts (ignoring the loop constraints).
Definition Contacts.cpp:493
std::vector< utils::Vector3d > constraintsInGlobal(const GeneralizedCoordinates &Q, bool updateKin)
Return the constraints position in the global reference.
Definition Contacts.cpp:284
An External force set that can apply forces to the model while computing the dynamics.
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau, bool updateKin=true)
Interface for the forward dynamics with contact of RBDL.
Definition Joints.cpp:1685
int getBodyRbdlIdToBiorbdId(const int idx) const
Return the Biorbd body identification from rbdl.
Definition Joints.cpp:258
utils::Matrix CalcPointJacobian(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointJacobian. Instead of sending the jacobian as an parameter,...
Definition Joints.cpp:508
utils::Vector3d CalcPointAcceleration(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointAcceleration. It is pretty much useless in Eigen, but fixes an issue in Ca...
Definition Joints.cpp:471
utils::Vector3d CalcPointVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity. It is pretty much useless in Eigen, but fixes an issue in Casadi...
Definition Joints.cpp:415
Joints & UpdateKinematicsCustom(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr)
Update the kinematic variables such as body velocities and accelerations in the model to reflect the ...
Definition Joints.cpp:1888
utils::Vector3d CalcBodyToBaseCoordinates(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcBodyToBaseCoordinates. It is pretty much useless in Eigen, but fixes an issue i...
Definition Joints.cpp:390
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
NodeSegment()
Construct a segment node.
const std::vector< bool > axes() const
A vector of bool which is the opposite of axesToRemove.
int parentId() const
Return the parent index.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
static String tolower(const String &str)
Convert a string to a lower case string.
Definition String.cpp:76
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21