Biorbd Release_1.13.0
Loading...
Searching...
No Matches
ExternalForceSet.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/ExternalForceSet.h"
3
4#include "BiorbdModel.h"
5#include "RigidBody/Contacts.h"
6#include "RigidBody/GeneralizedCoordinates.h"
7#include "RigidBody/GeneralizedVelocity.h"
8#include "RigidBody/Joints.h"
9#include "RigidBody/NodeSegment.h"
10#include "RigidBody/Segment.h"
11#include "RigidBody/SoftContactNode.h"
12#include "RigidBody/SoftContacts.h"
13#include "Utils/Error.h"
14#include "Utils/Rotation.h"
15#include "Utils/RotoTransNode.h"
16#include "Utils/SpatialVector.h"
17#include "Utils/String.h"
18#include "Utils/Vector3d.h"
19
20using namespace BIORBD_NAMESPACE;
21
23 Model& model,
24 bool useTranslationalForces,
25 bool useSoftContacts)
26 : m_model(model),
27 m_useTranslationalForces(useTranslationalForces),
28 m_useSoftContacts(useSoftContacts),
29 m_externalForces(std::vector<utils::SpatialVector>()),
33 std::vector<std::pair<utils::Vector3d, rigidbody::NodeSegment>>()) {
34 setZero();
35}
36
38
40 const utils::String& segmentName,
41 const utils::SpatialVector& vector) {
42 // Add 1 since 0 is used for the base ground
43 size_t dofIndex = m_model.segment(segmentName)
44 .getLastDofIndexInGeneralizedCoordinates(m_model) +
45 1;
46 m_externalForces[dofIndex] += vector;
47}
48
50 const utils::String& segmentName,
51 const utils::SpatialVector& vector,
52 const utils::Vector3d& pointOfApplication) {
54 vector,
56 utils::Vector3d(pointOfApplication, segmentName, segmentName)));
57 add(segmentName, atOrigin);
58}
59
61 const utils::String& segmentName,
62 const utils::SpatialVector& vector,
63 const utils::Vector3d& pointOfApplication) {
65 vector,
67 utils::RotoTrans(utils::Matrix3d::Identity(), pointOfApplication),
68 segmentName,
69 segmentName));
70}
71
73 const utils::Vector3d& force,
74 const utils::String& segmentName,
75 const utils::Vector3d& pointOfApplication) {
77 force,
79 utils::Vector3d(pointOfApplication, segmentName, segmentName)));
80}
81
83 const utils::Vector3d& force,
84 const rigidbody::NodeSegment& pointOfApplication) {
86 throw std::runtime_error(
87 "It is not possible to add translational forces if "
88 "useTranslationalForces was set to false");
89 m_translationalForces.push_back(std::make_pair(force, pointOfApplication));
90}
91
92std::vector<RigidBodyDynamics::Math::SpatialVector>
94 rigidbody::Joints& updatedModel) {
96 throw std::runtime_error(
97 "local reference frame requires Q when computing the Spatial "
98 "Vectors");
100 throw std::runtime_error(
101 "useTranslationalForce requires Q when computing the Spatial "
102 "Vectors");
104 throw std::runtime_error(
105 "useSoftContacts requires Q and Qdot when computing the Spatial "
106 "Vectors");
108 updatedModel,
110 rigidbody::GeneralizedVelocity(updatedModel));
111}
112
113std::vector<RigidBodyDynamics::Math::SpatialVector>
115 rigidbody::Joints& updatedModel,
118 throw std::runtime_error(
119 "useSoftContacts requires Qdot when computing the Spatial Vectors");
121 updatedModel, Q, rigidbody::GeneralizedVelocity(updatedModel));
122}
123
124std::vector<RigidBodyDynamics::Math::SpatialVector>
126 rigidbody::Joints& updatedModel,
128 const rigidbody::GeneralizedVelocity& Qdot) {
129 std::vector<utils::SpatialVector> tp(
130 computeSpatialVectors(updatedModel, Q, Qdot));
131 std::vector<RigidBodyDynamics::Math::SpatialVector> out;
132 for (const auto& value : tp) {
133 out.push_back(value);
134 }
135 return out;
136}
137
138std::vector<utils::SpatialVector>
140 rigidbody::Joints& updatedModel) {
142 throw std::runtime_error(
143 "local reference frame requires Q when computing the Spatial "
144 "Vectors");
146 throw std::runtime_error(
147 "useTranslationalForce requires Q when computing the Spatial "
148 "Vectors");
150 throw std::runtime_error(
151 "useSoftContacts requires Q and Qdot when computing the Spatial "
152 "Vectors");
154 updatedModel,
156 rigidbody::GeneralizedVelocity(updatedModel));
157}
158
159std::vector<utils::SpatialVector>
161 rigidbody::Joints& updatedModel,
164 throw std::runtime_error(
165 "useSoftContacts requires Qdot when computing the Spatial Vectors");
167 updatedModel, Q, rigidbody::GeneralizedVelocity());
168}
169
170std::vector<utils::SpatialVector>
172 rigidbody::Joints& updatedModel,
174 const rigidbody::GeneralizedVelocity& Qdot) {
175 std::vector<utils::SpatialVector> out;
176 for (const auto& value : m_externalForces) {
177 out.push_back(value);
178 }
180 combineLocalReferenceFrameForces(updatedModel, Q, out);
182 combineTranslationalForces(updatedModel, Q, out);
183 if (m_useSoftContacts) combineSoftContactForces(updatedModel, Q, Qdot, out);
184
185 return out;
186}
187
192
194 m_externalForces.clear();
195
196 // Null Spatial vector nul to fill the final table
197 utils::SpatialVector sv_zero(0., 0., 0., 0., 0., 0.);
198 m_externalForces.push_back(
199 sv_zero); // The first one is associated with the universe
200
201 // Dispatch the forces
202 for (size_t i = 0; i < m_model.nbSegment(); ++i) {
203 size_t nDof(m_model.segment(i).nbDof());
204 for (size_t i = 0; i < nDof; ++i) {
205 m_externalForces.push_back(sv_zero); // Put a sv_zero on each DoF
206 }
207 }
208
209 // Reset other elements of the class too
210 m_translationalForces.clear();
212}
213
215 rigidbody::Joints& updatedModel,
217 std::vector<utils::SpatialVector>& out) {
218 const auto& allGlobalJcs = updatedModel.allGlobalJCS(Q, false);
219
220 for (size_t i = 0; i < m_externalForcesInLocal.size(); i++) {
221 const auto& pair(m_externalForcesInLocal.get(static_cast<int>(i)));
222
223 // Aliases to have a better referencing of the variables
224 const utils::SpatialVector& vector(pair.first);
225 const utils::RotoTransNode& node(pair.second);
226
227 // Traverse the segment hierarchy from root to child to root until we
228 // get to a segment with at least one dof as it is not possible to add
229 // forces on segment without degree of freedom
230 const rigidbody::Segment* segment = &updatedModel.segment(node.parent());
231 utils::RotoTrans segmentRotoTrans = utils::RotoTrans::Identity();
232 do {
233 segmentRotoTrans *=
234 allGlobalJcs[updatedModel.getBodyBiorbdId(segment->name())];
235
236 if (segment->nbDof() > 0) break;
238 segment->parent().compare("root"),
239 node.parent() +
240 " should be attached to at least one segment "
241 "with a degree of freedom.");
242 segment = &updatedModel.segment(segment->parent());
243 } while (true);
244 const utils::RotoTransNode nodeInGrf(segmentRotoTrans * node);
245
246 const utils::Rotation& rotationInGrf(nodeInGrf.rot());
247 const utils::Vector3d& pointOfApplication(nodeInGrf.trans());
248
249 // Rotate the forces in global reference frame (Grf)
250 utils::Vector3d forceInGrf(vector.force());
251 forceInGrf.applyRT(rotationInGrf);
252
253 utils::Vector3d momentInGrf(vector.moment());
254 momentInGrf.applyRT(rotationInGrf);
255
256 // Transport the force to the global reference frame (Add 1 to account
257 // for the undeclared root)
258 size_t dofIndex =
259 updatedModel.segment(node.parent())
261 1;
262 out[dofIndex] += transportAtOrigin(
263 utils::SpatialVector(momentInGrf, forceInGrf), pointOfApplication);
264 }
265 return;
266}
267
269 rigidbody::Joints& updatedModel,
271 std::vector<utils::SpatialVector>& out) const {
272 // Do not waste time computing forces on empty vector
273 if (m_translationalForces.size() == 0) return;
274
275 for (auto& e : m_translationalForces) {
276 const rigidbody::NodeSegment& pointOfApplication = e.second;
277 const rigidbody::Segment& segment(
278 updatedModel.segment(pointOfApplication.parent()));
279 size_t dofIndex =
280 segment.getLastDofIndexInGeneralizedCoordinates(updatedModel) + 1;
281
282 const utils::Vector3d& force = e.first;
283 rigidbody::NodeSegment pointOfApplicationInGlobal(
284 updatedModel.CalcBodyToBaseCoordinates(
285 Q,
286 static_cast<unsigned int>(segment.id()),
287 pointOfApplication,
288 false),
289 pointOfApplication.Node::name(),
290 pointOfApplication.parent(),
291 pointOfApplication.isTechnical(),
292 pointOfApplication.isAnatomical(),
293 pointOfApplication.axesToRemoveAsString(),
294 pointOfApplication.parentId());
295
296 // Add the force to the force vector (do not subtract 1 because 0 is the
297 // base)
298 out[dofIndex] += transportForceAtOrigin(force, pointOfApplicationInGlobal);
299 }
300}
301
303 rigidbody::Joints& updatedModel,
306 std::vector<utils::SpatialVector>& out) const {
307 // Do not waste time computing forces on empty vector
308 if (m_model.nbSoftContacts() == 0) return;
309
310 for (size_t j = 0; j < m_model.nbSoftContacts(); j++) {
311 rigidbody::SoftContactNode& contact(m_model.softContact(j));
312 const rigidbody::Segment& segment(updatedModel.segment(contact.parent()));
313 size_t dofIndex =
314 segment.getLastDofIndexInGeneralizedCoordinates(updatedModel) + 1;
315
316 // Add the force to the force vector (do not subtract 1 because 0 is the
317 // base)
318 out[dofIndex] += contact.computeForceAtOrigin(updatedModel, Q, Qdot, false);
319 }
320}
321
323 const utils::Vector3d& force,
324 const rigidbody::NodeSegment& pointOfApplication) const {
325 utils::Vector3d forceTp;
326 if (pointOfApplication.nbAxesToRemove() > 0) {
327 // Fill only if direction is enabled
328 forceTp = utils::Vector3d(0., 0., 0.);
329 for (auto axis : pointOfApplication.availableAxesIndices()) {
330 forceTp.block(axis, 0, 1, 1) = force.block(axis, 0, 1, 1);
331 }
332 } else {
333 forceTp = force;
334 }
335
336 // Transport to Origin (Bour's formula)
337 utils::SpatialVector out(0., 0., 0., 0., 0., 0.);
338 out.block(0, 0, 3, 1) = forceTp.cross(-pointOfApplication);
339 out.block(3, 0, 3, 1) = forceTp;
340
341 return out;
342}
343
345 const utils::SpatialVector& v,
346 const rigidbody::NodeSegment& pointOfApplication) const {
347 // Transport to Origin (Bour's formula)
349 transportForceAtOrigin(v.force(), pointOfApplication));
350 out.block(0, 0, 3, 1) += v.moment();
351
352 return out;
353}
The actual musculoskeletal model that holds everything in biorbd.
This is an internal structure holding the information for the local forces. RotoTransNodes could have...
utils::SpatialVector transportAtOrigin(const utils::SpatialVector &v, const rigidbody::NodeSegment &pointOfApplication) const
Get the rigid contacts in a list of spatial vector of dimension 6xNdof.
bool hasExternalForceInLocalReferenceFrame() const
Return if there is at least one external force expressed in the local reference frame....
Model & m_model
A reference to the kinematic model.
std::vector< utils::SpatialVector > m_externalForces
The vector that holds all the external forces.
bool m_useSoftContacts
If soft contacts should be included.
void combineSoftContactForces(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, std::vector< utils::SpatialVector > &out) const
Add the soft contact forces to the internal Set.
void combineTranslationalForces(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, std::vector< utils::SpatialVector > &out) const
Add the translational forces to the internal Set.
std::vector< std::pair< utils::Vector3d, rigidbody::NodeSegment > > m_translationalForces
std::vector< RigidBodyDynamics::Math::SpatialVector > computeRbdlSpatialVectors(rigidbody::Joints &updatedModel)
The forces in a rbdl compatible format. This won't work if useTranslationalForces or useSoftContacts ...
bool m_useTranslationalForces
If translational forces should be included.
std::vector< utils::SpatialVector > computeSpatialVectors(rigidbody::Joints &updatedModel)
The forces in a rbdl compatible format. This won't work if useTranslationalForces or useSoftContacts ...
void setZero()
Initialize and zero out all the spatial vectors of the Set. Note this should always be called before ...
void addInSegmentReferenceFrame(const utils::String &segmentName, const utils::SpatialVector &vector, const utils::Vector3d &pointOfApplication)
Apply a new value to the specified spatial vector of the Set. WARNING: This vector is expected to be ...
void combineLocalReferenceFrameForces(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, std::vector< utils::SpatialVector > &out)
Add the forces expressed in the local reference to the internal Set.
ExternalForceSet(Model &model, bool useTranslationalForces=true, bool useSoftContacts=true)
Construct ExternalForceSet.
utils::SpatialVector transportForceAtOrigin(const utils::Vector3d &force, const rigidbody::NodeSegment &pointOfApplication) const
Get the rigid contacts in a list of spatial vector of dimension 6xNdof.
void add(const utils::String &segmentName, const utils::SpatialVector &vector)
Apply a new value to the specified spatial vector of the Set. WARNING: This vector is expected to be ...
void addTranslationalForce(const utils::Vector3d &forceInGlobal, const utils::String &segmentName, const utils::Vector3d &pointOfApplicationInLocal)
Add a pure force at a specific point on the kinematic chain.
~ExternalForceSet()
Delete the set and free the ressources.
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
std::vector< utils::RotoTrans > allGlobalJCS(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the joint coordinate system (JCS) in global reference frame at a given Q.
Definition Joints.cpp:343
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
int getBodyBiorbdId(const utils::String &segmentName) const
Return the biorbd body identification.
Definition Joints.cpp:246
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
utils::String axesToRemoveAsString() const
Return the axes to removed.
bool isTechnical() const
Return if node is technical.
std::vector< int > availableAxesIndices() const
Get all the indices of the axes to keep (X=0, Y=1, Z=2).
int parentId() const
Return the parent index.
bool isAnatomical() const
Return if node is anatomical.
int nbAxesToRemove() const
Return the number of axes to remove.
Description of a segment.
Definition Segment.h:29
size_t getLastDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the last index of the segment in the Generalized Coordinates vector.
Definition Segment.cpp:599
size_t id() const
Return the Segment index.
Definition Segment.cpp:196
size_t nbDof() const
Return the number of DoF of the segment.
Definition Segment.cpp:205
A point attached to a contact node, generally speaking a skin marker.
virtual utils::SpatialVector computeForceAtOrigin(Joints &model, const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Get the force in a spatial vector at the origin of the world base coordinates.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
const String & name() const
Return the name of the node.
Definition Node.cpp:43
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
A RotoTrans which is attached to a segment.
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).
utils::Vector3d moment() const
Return the moment part of the vector (first three elements).
utils::Vector3d force() const
Return the force part of the vector (last three elements).
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
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Definition Vector3d.cpp:79