1#define BIORBD_API_EXPORTS
2#include "RigidBody/ExternalForceSet.h"
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"
20using namespace BIORBD_NAMESPACE;
24 bool useTranslationalForces,
33 std::vector<std::pair<utils::Vector3d, rigidbody::
NodeSegment>>()) {
43 size_t dofIndex =
m_model.segment(segmentName)
44 .getLastDofIndexInGeneralizedCoordinates(
m_model) +
57 add(segmentName, atOrigin);
86 throw std::runtime_error(
87 "It is not possible to add translational forces if "
88 "useTranslationalForces was set to false");
92std::vector<RigidBodyDynamics::Math::SpatialVector>
96 throw std::runtime_error(
97 "local reference frame requires Q when computing the Spatial "
100 throw std::runtime_error(
101 "useTranslationalForce requires Q when computing the Spatial "
104 throw std::runtime_error(
105 "useSoftContacts requires Q and Qdot when computing the Spatial "
113std::vector<RigidBodyDynamics::Math::SpatialVector>
118 throw std::runtime_error(
119 "useSoftContacts requires Qdot when computing the Spatial Vectors");
124std::vector<RigidBodyDynamics::Math::SpatialVector>
129 std::vector<utils::SpatialVector> tp(
131 std::vector<RigidBodyDynamics::Math::SpatialVector> out;
132 for (
const auto& value : tp) {
133 out.push_back(value);
138std::vector<utils::SpatialVector>
142 throw std::runtime_error(
143 "local reference frame requires Q when computing the Spatial "
146 throw std::runtime_error(
147 "useTranslationalForce requires Q when computing the Spatial "
150 throw std::runtime_error(
151 "useSoftContacts requires Q and Qdot when computing the Spatial "
159std::vector<utils::SpatialVector>
164 throw std::runtime_error(
165 "useSoftContacts requires Qdot when computing the Spatial Vectors");
170std::vector<utils::SpatialVector>
175 std::vector<utils::SpatialVector> out;
177 out.push_back(value);
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) {
217 std::vector<utils::SpatialVector>& out) {
218 const auto& allGlobalJcs = updatedModel.
allGlobalJCS(Q,
false);
236 if (segment->
nbDof() > 0)
break;
238 segment->
parent().compare(
"root"),
240 " should be attached to at least one segment "
241 "with a degree of freedom.");
251 forceInGrf.
applyRT(rotationInGrf);
254 momentInGrf.
applyRT(rotationInGrf);
271 std::vector<utils::SpatialVector>& out)
const {
286 static_cast<unsigned int>(segment.
id()),
289 pointOfApplication.Node::name(),
290 pointOfApplication.
parent(),
306 std::vector<utils::SpatialVector>& out)
const {
308 if (
m_model.nbSoftContacts() == 0)
return;
310 for (
size_t j = 0; j <
m_model.nbSoftContacts(); j++) {
330 forceTp.block(axis, 0, 1, 1) = force.block(axis, 0, 1, 1);
338 out.block(0, 0, 3, 1) = forceTp.cross(-pointOfApplication);
339 out.block(3, 0, 3, 1) = forceTp;
350 out.block(0, 0, 3, 1) += v.
moment();
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 ...
LocalForcesInternal m_externalForcesInLocal
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.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
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.
Segment & segment(size_t idx)
Get a segment of index idx.
int getBodyBiorbdId(const utils::String &segmentName) const
Return the biorbd body identification.
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...
A point attached to a segment, generally speaking a skin marker.
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.
size_t getLastDofIndexInGeneralizedCoordinates(const rigidbody::Joints &model) const
Returns the last index of the segment in the Generalized Coordinates vector.
size_t id() const
Return the Segment index.
size_t nbDof() const
Return the number of DoF of the segment.
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.
const String & name() const
Return the name of the node.
const String & parent() const
Return the parent name of the node.
A RotoTrans which is attached to a segment.
Homogenous matrix to describe translations and rotations simultaneously.
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.
Wrapper around Eigen Vector3d and attach it to a parent.
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.