1#define BIORBD_API_EXPORTS
2#include "RigidBody/Contacts.h"
4#include <rbdl/Kinematics.h>
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"
23using namespace BIORBD_NAMESPACE;
26 : RigidBodyDynamics::ConstraintSet(),
39 static_cast<RigidBodyDynamics::ConstraintSet&
>(*this) = other;
52#ifndef BIORBD_USE_CASADI_MATH
55 worldNormal.norm() == 1.0,
"Normal of the contact must be provided");
64 SCALAR_TO_DOUBLE(has_x, worldNormal[0]);
65 SCALAR_TO_DOUBLE(has_y, worldNormal[1]);
66 SCALAR_TO_DOUBLE(has_z, worldNormal[2]);
84 static_cast<int>(bodyId)));
85 return static_cast<size_t>(
86 RigidBodyDynamics::ConstraintSet::AddContactConstraint(
87 static_cast<unsigned int>(bodyId),
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),
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),
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),
122 (name +
"_Z").c_str()));
135 static_cast<int>(bodyId)));
140 size_t body_id_predecessor,
141 size_t body_id_successor,
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()),
177 bool resolveAllInRootFrame =
true;
180 std::vector<unsigned int> constraintBodyIdsOutput;
181 std::vector<RigidBodyDynamics::Math::SpatialTransform>
182 updatedConstraintBodyFramesOutput;
183 std::vector<RigidBodyDynamics::Math::SpatialVector>
184 updatedConstraintForcesOutput;
190#ifdef BIORBD_USE_CASADI_MATH
199 Q, Qdot, Tau, CS, externalForces,
false);
201 std::vector<utils::SpatialVector> out;
203 constraintBodyIdsOutput.clear();
204 updatedConstraintBodyFramesOutput.clear();
205 updatedConstraintForcesOutput.clear();
212 constraintBodyIdsOutput,
213 updatedConstraintBodyFramesOutput,
214 updatedConstraintForcesOutput,
215 resolveAllInRootFrame,
220 out.push_back(updatedConstraintForcesOutput[0]);
260 std::vector<utils::String> names;
261 for (
auto name : RigidBodyDynamics::ConstraintSet::name) {
262 names.push_back(name);
270 return RigidBodyDynamics::ConstraintSet::name[i];
281 throw std::runtime_error(
"Contact '" + name +
"' not found.");
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) {
296 contactConstraints[i]->getBodyIds()[0],
297 contactConstraints[i]->getBodyFrames()[0].r,
324 std::vector<utils::Vector3d> tp;
357 std::vector<utils::Vector3d> tp;
378 Q, Qdot, Qddot, c.
parentId(), c, updateKin);
390 std::vector<utils::Vector3d> tp;
393 Q, Qdot, Qddot, c.parentId(), c, updateKin));
403 const auto axes = contact.
axes();
412 for (
size_t i = 0; i < 3; ++i) {
414 Jout.block(r++, 0, 1, J.cols()) = J.block(i, 0, 1, J.cols());
438#ifdef BIORBD_USE_CASADI_MATH
445 updateKin ? &Q :
nullptr);
453#ifdef BIORBD_USE_CASADI_MATH
460 updateKin ? &Q :
nullptr);
468 std::vector<utils::Matrix> G;
500 "Idx for rigid contact Segment Id is too high..");
511 int segment_idx)
const {
513 std::vector<size_t> indices;
518 indices.push_back(i);
528 std::string reference =
"xyz";
534 std::back_inserter(out));
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.
utils::Vector3d rigidContactVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the velocity of the chosen contact.
std::shared_ptr< std::vector< rigidbody::NodeSegment > > m_rigidContacts
std::vector< utils::String > contactNames()
Return the name of the all contacts.
std::shared_ptr< size_t > m_nbreConstraint
Number of constraints.
utils::Matrix rigidContactJacobian(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q, size_t idx)
Compute the Jacobian of a single rigid contact.
size_t nbLoopConstraints() const
Return the number of loop constraints.
int contactSegmentBiorbdId(int idx) const
Return the segment idx of the contact in biorbd formalism.
static utils::Matrix projectContactJacobian(const utils::Matrix &J, const rigidbody::NodeSegment &contact)
Project a 3D point Jacobian onto active contact axes.
std::shared_ptr< size_t > m_nbLoopConstraint
Number of constraints.
size_t contactRbdlId(const utils::String &name)
Return the index of the contact corresponding to the given name.
utils::Vector getForce() const
Return the force acting on the constraint.
bool hasContacts() const
Check if there are contacts.
Contacts DeepCopy() const
Deep copy of contacts.
Contacts & getConstraints()
Get constraints.
bool hasLoopConstraints() const
Check if there are loop constraints.
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.
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.
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.
size_t nbContacts() const
Return the number of contacts.
std::vector< utils::Vector3d > rigidContactsVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocities of all the contacts.
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...
virtual ~Contacts()
Destroy the class properly.
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.
const NodeSegment & rigidContact(size_t idx) const
Returns the rigid contact idx as declared in the model.
std::vector< size_t > segmentRigidContactIdx(int segment_idx) const
Return the index of rigid contacts for a specified segment index.
std::vector< utils::Matrix > rigidContactsJacobian(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates &Q)
Compute the Jacobians of all rigid contacts.
std::shared_ptr< bool > m_isBinded
If the model is ready.
utils::String contactName(size_t i)
Return the name of the contact of a specified axis.
std::vector< utils::SpatialVector > calcLoopConstraintForces(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedTorque &Tau)
compute the forces generated from loop constraints
Contacts()
Construct contacts.
int nbRigidContacts() const
Returns the number of rigid contacts (ignoring the loop constraints).
std::vector< utils::Vector3d > constraintsInGlobal(const GeneralizedCoordinates &Q, bool updateKin)
Return the constraints position in the global reference.
An External force set that can apply forces to the model while computing the dynamics.
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau, bool updateKin=true)
Interface for the forward dynamics with contact of RBDL.
int getBodyRbdlIdToBiorbdId(const int idx) const
Return the Biorbd body identification from rbdl.
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,...
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...
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...
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 ...
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.
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.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
A wrapper for the Eigen::MatrixXd.
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).
Wrapper around the std::string class with augmented functionality.
static String tolower(const String &str)
Convert a string to a lower case string.
Wrapper around Eigen Vector3d and attach it to a parent.
Wrapper of the Eigen VectorXd.