Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Contacts.h
1#ifndef BIORBD_RIGIDBODY_CONTACTS_H
2#define BIORBD_RIGIDBODY_CONTACTS_H
3
4#include "biorbdConfig.h"
5
6#include <memory>
7#include <vector>
8
9#include <rbdl/Constraints.h>
10
11namespace BIORBD_NAMESPACE {
12namespace utils {
13class RotoTrans;
14class Vector3d;
15class Vector;
16class String;
17class SpatialVector;
18class Matrix;
19} // namespace utils
20
21namespace rigidbody {
27class NodeSegment;
28class Joints;
29
33#ifdef SWIG
34class BIORBD_API Contacts
35#else
36class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet
37#endif
38{
39 public:
43 Contacts();
44
49 Contacts DeepCopy() const;
50
55 void DeepCopy(const Contacts& other);
56
69 size_t AddConstraint(
70 size_t bodyId,
71 const utils::Vector3d& bodyPoint,
72 const utils::Vector3d& worldNormal,
73 const utils::String& name,
74 const utils::String& parentName);
75
87
88 size_t AddConstraint(
89 size_t bodyId,
90 const utils::Vector3d& bodyPoint,
91 const utils::String& axis,
92 const utils::String& name,
93 const utils::String& parentName);
94
111 size_t AddLoopConstraint(
112 size_t body_id_predecessor,
113 size_t body_id_successor,
114 const utils::RotoTrans& X_predecessor,
115 const utils::RotoTrans& X_successor,
116 const utils::SpatialVector& axis,
117 const utils::String& name,
118 bool enableStabilization = false,
119 double stabilizationParam = 0.1);
120
129 std::vector<utils::SpatialVector> calcLoopConstraintForces(
133
143 std::vector<utils::SpatialVector> calcLoopConstraintForces(
147 rigidbody::ExternalForceSet& externalForces);
148
152 virtual ~Contacts();
153
159
164 bool hasContacts() const;
165
170 bool hasLoopConstraints() const;
171
176 size_t nbContacts() const;
177
182 size_t nbLoopConstraints() const;
183
188 std::vector<utils::String> contactNames();
189
195 utils::String contactName(size_t i);
196
203 size_t contactRbdlId(const utils::String& name);
204
212 std::vector<utils::Vector3d> constraintsInGlobal(
213 const GeneralizedCoordinates& Q,
214 bool updateKin);
215
220 utils::Vector getForce() const;
221
227 int contactSegmentBiorbdId(int idx) const;
228
234 std::vector<size_t> segmentRigidContactIdx(int segment_idx) const;
235
241 int nbRigidContacts() const;
242
247 const std::vector<NodeSegment>& rigidContacts() const;
248
254 const NodeSegment& rigidContact(size_t idx) const;
255
265 rigidContact(const GeneralizedCoordinates& Q, size_t idx, bool updateKin);
266
274 std::vector<utils::Vector3d> rigidContacts(
275 const GeneralizedCoordinates& Q,
276 bool updateKin);
277
289 size_t idx,
290 bool updateKin = true);
291
299 std::vector<utils::Vector3d> rigidContactsVelocity(
302 bool updateKin = true);
303
317 size_t idx,
318 bool updateKin = true);
319
328 std::vector<utils::Vector3d> rigidContactsAcceleration(
332 bool updateKin = true);
333
342 rigidbody::Joints& updatedModel,
344 size_t idx);
345
355 size_t idx,
356 bool updateKin = true);
357
365 std::vector<utils::Matrix> rigidContactsJacobian(
366 rigidbody::Joints& updatedModel,
368
376 std::vector<utils::Matrix> rigidContactsJacobian(
378 bool updateKin = true);
379
380 protected:
388 utils::String swapAxes(const utils::String& axesToSwap) const;
389
390 std::shared_ptr<size_t> m_nbreConstraint;
391 std::shared_ptr<bool> m_isBinded;
392 std::shared_ptr<std::vector<rigidbody::NodeSegment>>
395 std::shared_ptr<size_t> m_nbLoopConstraint;
396
404 const utils::Matrix& J,
405 const rigidbody::NodeSegment& contact);
406};
407
408} // namespace rigidbody
409} // namespace BIORBD_NAMESPACE
410
411#endif // BIORBD_RIGIDBODY_CONTACTS_H
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
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
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
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
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21