1#define BIORBD_API_EXPORTS
2#include "RigidBody/Joints.h"
4#include <rbdl/Dynamics.h>
5#include <rbdl/Kinematics.h>
6#include <rbdl/rbdl_utils.h>
8#include "BiorbdModel.h"
9#include "RigidBody/Contacts.h"
10#include "RigidBody/ExternalForceSet.h"
11#include "RigidBody/GeneralizedAcceleration.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedTorque.h"
14#include "RigidBody/GeneralizedVelocity.h"
15#include "RigidBody/Markers.h"
16#include "RigidBody/Mesh.h"
17#include "RigidBody/MeshFace.h"
18#include "RigidBody/NodeSegment.h"
19#include "RigidBody/Segment.h"
20#include "RigidBody/SegmentCharacteristics.h"
21#include "RigidBody/SoftContacts.h"
22#include "Utils/Error.h"
23#include "Utils/Matrix.h"
24#include "Utils/Matrix3d.h"
25#include "Utils/Quaternion.h"
26#include "Utils/Rotation.h"
27#include "Utils/RotoTrans.h"
28#include "Utils/Scalar.h"
29#include "Utils/SpatialTransform.h"
30#include "Utils/SpatialVector.h"
31#include "Utils/String.h"
33using namespace BIORBD_NAMESPACE;
36 : RigidBodyDynamics::
Model(),
38 m_nbRoot(std::make_shared<size_t>(0)),
39 m_nbDof(std::make_shared<size_t>(0)),
40 m_nbQ(std::make_shared<size_t>(0)),
41 m_nbQdot(std::make_shared<size_t>(0)),
51 : RigidBodyDynamics::
Model(other),
71 static_cast<RigidBodyDynamics::Model &
>(*this) = other;
73 for (
size_t i = 0; i < other.
m_segments->size(); ++i) {
90 std::vector<utils::String> names;
91 for (
size_t i = 0; i <
nbSegment(); ++i) {
92 for (
size_t j = 0; j <
segment(i).nbDof(); ++j) {
97 for (
size_t i = 0; i <
nbSegment(); ++i) {
98 if (
segment(i).isRotationAQuaternion()) {
117 const std::vector<utils::Range> &QRanges,
118 const std::vector<utils::Range> &QdotRanges,
119 const std::vector<utils::Range> &QddotRanges,
120 const std::vector<utils::Scalar> &jointDampings,
135 referenceFrame.
rot().transpose(), referenceFrame.
trans()));
136 if (this->GetBodyId(parentName.c_str()) ==
137 std::numeric_limits<unsigned int>::max()) {
151 characteristics.mMass;
160 const std::vector<utils::Range> &QRanges,
161 const std::vector<utils::Range> &QdotRanges,
162 const std::vector<utils::Range> &QddotRanges,
163 const std::vector<utils::Scalar> &jointDampings,
177 referenceFrame.
rot().transpose(), referenceFrame.
trans()));
178 if (this->GetBodyId(parentName.c_str()) ==
179 std::numeric_limits<unsigned int>::max()) {
186 characteristics.mMass;
194 gravity = newGravity;
201 idx < m_segments->size(),
"Asked for a wrong segment (out of range)");
202 (*m_segments)[idx].updateCharacteristics(*
this, characteristics);
207 idx < m_segments->size(),
"Asked for a wrong segment (out of range)");
217 idx < m_segments->size(),
"Asked for a wrong segment (out of range)");
236 for (
auto &damping :
segment.jointDampings()) {
237 dampings[count] = damping * Qdot[count];
247 for (
size_t i = 0; i <
m_segments->size(); ++i)
248 if (!(*
m_segments)[i].name().compare(segmentName)) {
249 return static_cast<int>(i);
255 return GetBodyId(segmentName.c_str());
262 std::string bodyName = model.GetBodyName(idx);
272 std::vector<std::vector<size_t>> subTrees;
273 std::vector<size_t> subTree_empty;
274 for (
size_t j = 0; j < this->mu.size(); ++j) {
275 subTrees.push_back(subTree_empty);
279 std::vector<size_t> dof_with_no_parent_id;
280 for (
size_t i = 1; i < this->mu.size();
282 if (this->lambda[i] == 0) {
283 dof_with_no_parent_id.push_back(i);
288 for (
size_t i = 0; i < dof_with_no_parent_id.size(); ++i) {
289 size_t dof_id = dof_with_no_parent_id[i];
292 std::vector<std::vector<size_t>> subTrees_temp;
293 for (
size_t j = 0; j < this->mu.size(); ++j) {
294 subTrees_temp.push_back(subTree_empty);
297 std::vector<std::vector<size_t>> subTrees_temp_filled =
299 for (
size_t j = 0; j < subTrees_temp.size(); ++j) {
300 if (subTrees_temp_filled[j].empty()) {
305 subTrees_temp_filled[j].begin(),
306 subTrees_temp_filled[j].end());
311 subTrees.erase(subTrees.begin());
317 std::vector<std::vector<size_t>> subTrees,
319 size_t q_index_i = this->mJoints[idx].q_index;
320 subTrees[idx].push_back(q_index_i);
322 std::vector<std::vector<size_t>> subTrees_filled;
323 subTrees_filled = subTrees;
325 std::vector<unsigned int> child_idx = this->mu[idx];
327 if (child_idx.size() > 0) {
328 for (
size_t i = 0; i < child_idx.size(); ++i) {
329 size_t cur_child_id = child_idx[i];
331 std::vector<size_t> subTree_child = subTrees_filled[cur_child_id];
333 subTrees_filled[idx].insert(
334 subTrees_filled[idx].end(),
335 subTree_child.begin(),
336 subTree_child.end());
340 return subTrees_filled;
346#ifdef BIORBD_USE_CASADI_MATH
353 std::vector<utils::RotoTrans> out;
354 for (
size_t i = 0; i <
m_segments->size(); ++i) {
355 out.push_back(model.globalJCS(Q, i,
false));
376 std::vector<utils::RotoTrans> out;
377 for (
size_t i = 0; i <
m_segments->size(); ++i) {
396 Q, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
404#ifdef BIORBD_USE_CASADI_MATH
411 return RigidBodyDynamics::CalcBodyToBaseCoordinates(
412 model, Q, bodyId, pointInLocal,
false);
422 Q, Qdot, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
431#ifdef BIORBD_USE_CASADI_MATH
437 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
439 return RigidBodyDynamics::CalcPointVelocity(
440 model, Q, Qdot, bodyId, pointInLocal,
false);
450 Q, Qdot, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
459#ifdef BIORBD_USE_CASADI_MATH
465 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
467 return RigidBodyDynamics::CalcPointVelocity6D(
468 model, Q, Qdot, bodyId, pointInLocal,
false);
482 this->GetBodyId(segmentName.c_str()),
494#ifdef BIORBD_USE_CASADI_MATH
500 updateKin ? &Q :
nullptr,
501 updateKin ? &Qdot :
nullptr,
502 updateKin ? &Qddot :
nullptr);
504 return RigidBodyDynamics::CalcPointAcceleration(
505 model, Q, Qdot, Qddot, bodyId, pointInLocal,
false);
514 Q, this->GetBodyId(segmentName.c_str()), pointInLocal, updateKin);
522#ifdef BIORBD_USE_CASADI_MATH
531 RigidBodyDynamics::CalcPointJacobian(
532 model, Q, bodyId, pointInLocal, out,
false);
538 const std::vector<NodeSegment> &v,
546 "Number of marker must be equal to number of Vector3d");
548#ifdef BIORBD_USE_CASADI_MATH
555 std::vector<rigidbody::NodeSegment> out;
556 for (
size_t i = 0; i < marks.
nbMarkers(); ++i) {
562 updatedModel.globalJCS(Q, tp.
parent(),
false).transpose());
564 out.push_back(updatedModel.projectPoint(Q, pos,
false));
580#ifdef BIORBD_USE_CASADI_MATH
589 segment(
static_cast<size_t>(segmentIdx)).name());
592 updatedModel.globalJCS(Q,
static_cast<size_t>(segmentIdx),
false)
599 static_cast<int>(GetBodyId(segmentName.c_str())));
602 return updatedModel.projectPoint(Q, node,
false);
611 Q, n, updateKin,
true);
621 return utils::Matrix::Zero(3,
static_cast<unsigned int>(
nbQ()));
624#ifdef BIORBD_USE_CASADI_MATH
635 node.
applyRT(updatedModel.globalJCS(Q, node.
parent(),
false).transpose());
639 utils::Matrix JCor(utils::Matrix::Zero(9,
static_cast<unsigned int>(
nbQ())));
640 updatedModel.CalcMatRotJacobian(
642 GetBodyId(node.
parent().c_str()),
643 utils::Matrix3d::Identity(),
646 for (
size_t n = 0; n < 3; ++n) {
649 static_cast<unsigned int>(n) * 3,
652 static_cast<unsigned int>(
nbQ())) *
653 node(
static_cast<unsigned int>(n));
666#ifdef BIORBD_USE_CASADI_MATH
675 updatedModel.projectPoint(Q, v, segmentIdx, axesToRemove,
false));
679 return updatedModel.projectPointJacobian(Q, p,
false);
684 const std::vector<NodeSegment> &v,
686#ifdef BIORBD_USE_CASADI_MATH
694 const std::vector<rigidbody::NodeSegment> &tp(
695 updatedModel.projectPoint(Q, v,
false));
699 std::vector<utils::Matrix> G;
701 for (
size_t i = 0; i < tp.size(); ++i) {
703 G.push_back(updatedModel.projectPointJacobian(
711 const size_t segmentIdx,
713#ifdef BIORBD_USE_CASADI_MATH
720 if (segmentIdx >= updatedModel.fixed_body_discriminator) {
722 segmentIdx -
static_cast<size_t>(updatedModel.fixed_body_discriminator);
724 static_cast<size_t>(updatedModel.mFixedBodies[fbody_id].mMovableParent);
726 updatedModel.X_base[parent_id].E.transpose(),
727 updatedModel.X_base[parent_id].r);
730 updatedModel.mFixedBodies[fbody_id].mParentTransform.E.transpose(),
731 updatedModel.mFixedBodies[fbody_id].mParentTransform.r),
739 updatedModel.X_base[segmentIdx].E.transpose(),
740 updatedModel.X_base[segmentIdx].r);
751 ->CalcPointVelocity6D(
761 const std::vector<rigidbody::NodeSegment> &com_segment(
764 for (
size_t i = 0; i < com_segment.size(); ++i) {
765 com += (*m_segments)[i].characteristics().mMass * com_segment[i];
769 com = com / this->
mass();
785#ifdef BIORBD_USE_CASADI_MATH
793 static_cast<unsigned int>(
nbQ()),
static_cast<unsigned int>(
nbQ()));
795 RigidBodyDynamics::CompositeRigidBodyAlgorithm(
803#ifdef BIORBD_USE_CASADI_MATH
812 utils::Matrix Minv(updatedModel.dof_count, updatedModel.dof_count);
816 for (i = 1; i < updatedModel.mBodies.size(); i++) {
817 updatedModel.I[i].setSpatialMatrix(updatedModel.IA[i]);
825 std::vector<utils::Matrix> F;
826 for (i = 1; i < updatedModel.mBodies.size(); i++) {
832 for (i =
static_cast<int>(updatedModel.mBodies.size() - 1); i > 0; i--) {
833 unsigned int q_index_i =
834 static_cast<size_t>(updatedModel.mJoints[i].q_index);
835 const std::vector<size_t> &sub_tree = subTrees[q_index_i];
837 updatedModel.U[i] = updatedModel.IA[i] * updatedModel.S[i];
838 updatedModel.d[i] = updatedModel.S[i].dot(updatedModel.U[i]);
840 Minv(q_index_i, q_index_i) = 1.0 / (updatedModel.d[i]);
842 for (j = 0; j < sub_tree.size(); j++) {
844 F[q_index_i].block(0,
static_cast<unsigned int>(sub_tree[j]), 6, 1));
845 Minv(q_index_i,
static_cast<unsigned int>(sub_tree[j])) -=
846 (1.0 / updatedModel.d[i]) * updatedModel.S[i].transpose() * Ftemp;
849 size_t lambda =
static_cast<size_t>(updatedModel.lambda[i]);
851 static_cast<size_t>(updatedModel.mJoints[lambda].q_index);
853 for (j = 0; j < sub_tree.size(); j++) {
854 F[q_index_i].block(0,
static_cast<unsigned int>(sub_tree[j]), 6, 1) +=
856 Minv.block(q_index_i,
static_cast<unsigned int>(sub_tree[j]), 1, 1);
858 F[lambda_q_i].block(0,
static_cast<unsigned int>(sub_tree[j]), 6, 1) +=
859 updatedModel.X_lambda[i].toMatrixTranspose() *
860 F[q_index_i].block(0,
static_cast<unsigned int>(sub_tree[j]), 6, 1);
863 RigidBodyDynamics::Math::SpatialMatrix Ia =
866 (updatedModel.U[i] / updatedModel.d[i]).transpose();
868#ifdef BIORBD_USE_CASADI_MATH
869 updatedModel.IA[lambda] += updatedModel.X_lambda[i].toMatrixTranspose() *
870 Ia * updatedModel.X_lambda[i].toMatrix();
873 updatedModel.IA[lambda].noalias() +=
874 updatedModel.X_lambda[i].toMatrixTranspose() * Ia *
875 updatedModel.X_lambda[i].toMatrix();
882 for (i = 1; i < updatedModel.mBodies.size(); i++) {
883 unsigned int q_index_i = updatedModel.mJoints[i].q_index;
884 unsigned int lambda = updatedModel.lambda[i];
885 unsigned int lambda_q_i = updatedModel.mJoints[lambda].q_index;
890 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
892 Minv(q_index_i, j) -=
893 (1.0 / updatedModel.d[i]) *
894 (updatedModel.U[i].transpose() * X_lambda.toMatrix()) * Ftemp;
899 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
900 F[q_index_i].block(0, j, 6, 1) =
901 updatedModel.S[i] * Minv.block(q_index_i, j, 1, 1);
906 for (j = q_index_i; j < static_cast<int>(updatedModel.dof_count); j++) {
907 F[q_index_i].block(0, j, 6, 1) +=
908 X_lambda.toMatrix() * F[lambda_q_i].block(0, j, 6, 1);
914 for (j = 0; j < static_cast<int>(updatedModel.dof_count); j++) {
915 for (i = 0; i < static_cast<int>(updatedModel.dof_count); i++) {
917 Minv(i, j) = Minv(j, i);
929#ifdef BIORBD_USE_CASADI_MATH
935 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
943 Q,
segment.name(),
segment.characteristics().mCenterOfMass,
false);
944 com_dot += ((Jac * Qdot) *
segment.characteristics().mMass);
947 com_dot = com_dot /
mass();
979#ifdef BIORBD_USE_CASADI_MATH
987 utils::Matrix JacTotal(utils::Matrix::Zero(3, this->dof_count));
992 Q,
segment.name(),
segment.characteristics().mCenterOfMass,
false);
993 JacTotal +=
segment.characteristics().mMass * Jac;
997 JacTotal /= this->
mass();
1007 utils::Scalar &
mass,
1014#ifdef BIORBD_USE_CASADI_MATH
1020 updateKin ? &Q :
nullptr,
1021 updateKin ? &Qdot :
nullptr,
1022 updateKin ? Qddot :
nullptr);
1024 RigidBodyDynamics::Utils::CalcCenterOfMass(
1034 changeOfAngularMomentum,
1059#ifdef BIORBD_USE_CASADI_MATH
1065 updateKin ? &Q :
nullptr,
1066 updateKin ? &Qdot :
nullptr,
1067 updateKin ? &Qddot :
nullptr);
1070 RigidBodyDynamics::Utils::CalcZeroMomentPoint(
1071 updatedModel, Q, Qdot, Qddot, &zmp, normal, point,
false);
1073 zmp += normal * ((point - zmp).dot(normal) / normal.dot(normal));
1081#ifdef BIORBD_USE_CASADI_MATH
1088 std::vector<rigidbody::NodeSegment> out;
1089 for (
size_t i = 0; i <
m_segments->size(); ++i) {
1090 out.push_back(updatedModel.CoMbySegment(Q, i,
false));
1098 std::vector<rigidbody::NodeSegment> allCoM(
CoMbySegment(Q, updateKin));
1101 for (
size_t i = 0; i < allCoM.size(); ++i) {
1102 CoMs.block(0,
static_cast<unsigned int>(i), 3, 1) = allCoM[i];
1112 idx < m_segments->size(),
"Choosen segment doesn't exist");
1116 static_cast<unsigned int>((*
m_segments)[idx].
id()),
1117 (*
m_segments)[idx].characteristics().mCenterOfMass,
1125#ifdef BIORBD_USE_CASADI_MATH
1131 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
1133 std::vector<utils::Vector3d> out;
1134 for (
size_t i = 0; i <
m_segments->size(); ++i) {
1135 out.push_back(updatedModel.CoMdotBySegment(Q, Qdot, i,
false));
1147 idx < m_segments->size(),
"Choosen segment doesn't exist");
1152 static_cast<unsigned int>((*
m_segments)[idx].
id()),
1153 (*
m_segments)[idx].characteristics().mCenterOfMass,
1162#ifdef BIORBD_USE_CASADI_MATH
1168 updateKin ? &Q :
nullptr,
1169 updateKin ? &Qdot :
nullptr,
1170 updateKin ? &Qddot :
nullptr);
1172 std::vector<utils::Vector3d> out;
1173 for (
size_t i = 0; i <
m_segments->size(); ++i) {
1174 out.push_back(updatedModel.CoMddotBySegment(Q, Qdot, Qddot, i,
false));
1186 idx < m_segments->size(),
"Choosen segment doesn't exist");
1192 static_cast<unsigned int>((*
m_segments)[idx].
id()),
1193 (*
m_segments)[idx].characteristics().mCenterOfMass,
1200#ifdef BIORBD_USE_CASADI_MATH
1208 const std::vector<utils::RotoTrans> &RT(updatedModel.allGlobalJCS(Q,
false));
1211 std::vector<std::vector<utils::Vector3d>> out;
1212 for (
size_t i = 0; i <
nbSegment(); ++i) {
1213 out.push_back(updatedModel.meshPoints(RT, i));
1222 const std::vector<utils::RotoTrans> &RT(
allGlobalJCS(Q, updateKin));
1230 const std::vector<utils::RotoTrans> &RT(
allGlobalJCS(Q, updateKin));
1232 std::vector<utils::Matrix> all_points;
1233 for (
size_t i = 0; i <
m_segments->size(); ++i) {
1235 for (
size_t j = 0; j <
mesh(i).nbVertex(); ++j) {
1238 mat.block(0,
static_cast<unsigned int>(j), 3, 1) = tp;
1240 all_points.push_back(mat);
1245 const std::vector<utils::RotoTrans> &RT,
1248 std::vector<utils::Vector3d> v;
1249 for (
size_t j = 0; j <
mesh(i).nbVertex(); ++j) {
1261 std::vector<std::vector<rigidbody::MeshFace>> v_all;
1262 for (
size_t j = 0; j <
nbSegment(); ++j) {
1270 return mesh(idx).faces();
1274 std::vector<rigidbody::Mesh> segmentOut;
1275 for (
size_t i = 0; i <
nbSegment(); ++i) {
1276 segmentOut.push_back(
mesh(i));
1282 return segment(idx).characteristics().mesh();
1286 for (
size_t i = 0; i <
nbSegment(); ++i) {
1287 segment(i).characteristics().mesh().path().setFolder(folder);
1336#ifdef BIORBD_USE_CASADI_MATH
1342 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr,
nullptr);
1346 RigidBodyDynamics::Utils::CalcCenterOfMass(
1360 std::vector<utils::Vector3d> h_segment;
1361 for (
size_t i = 1; i < updatedModel.mBodies.size(); i++) {
1362 updatedModel.Ic[i] = updatedModel.I[i];
1363 updatedModel.hc[i] = updatedModel.Ic[i].toMatrix() * updatedModel.v[i];
1366 updatedModel.X_lambda[i].applyTranspose(updatedModel.hc[i]);
1367 if (updatedModel.lambda[i] != 0) {
1370 j = updatedModel.lambda[j];
1371 h = updatedModel.X_lambda[j].applyTranspose(h);
1372 }
while (updatedModel.lambda[j] != 0);
1374 h = X_to_COM.applyAdjoint(h);
1386#ifdef BIORBD_USE_CASADI_MATH
1392 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr,
nullptr);
1396 RigidBodyDynamics::Utils::CalcCenterOfMass(
1410 std::vector<utils::Vector3d> h_segment;
1411 for (
size_t i = 1; i < updatedModel.mBodies.size(); i++) {
1412 updatedModel.Ic[i] = updatedModel.I[i];
1413 updatedModel.hc[i] = updatedModel.Ic[i].toMatrix() * updatedModel.v[i];
1416 updatedModel.X_lambda[i].applyTranspose(updatedModel.hc[i]);
1417 if (updatedModel.lambda[i] != 0) {
1420 j = updatedModel.lambda[j];
1421 h = updatedModel.X_lambda[j].applyTranspose(h);
1422 }
while (updatedModel.lambda[j] != 0);
1424 h = X_to_COM.applyAdjoint(h);
1436 const utils::Scalar &k_stab) {
1443 unsigned int cmpQuat(0);
1444 unsigned int cmpDof(0);
1445 for (
size_t i = 0; i <
nbSegment(); ++i) {
1450 Q(Q.size() -
static_cast<unsigned int>(*
m_nRotAQuat + cmpQuat)),
1452 cmpDof +
static_cast<unsigned int>(segment_i.
nbDofTrans()),
1460 cmpDof, 0,
static_cast<unsigned int>(segment_i.
nbDofTrans()), 1) =
1462 cmpDof, 0,
static_cast<unsigned int>(segment_i.
nbDofTrans()), 1);
1466 cmpDof +
static_cast<unsigned int>(segment_i.
nbDofTrans()), 0, 3, 1));
1468 cmpDof +
static_cast<unsigned int>(segment_i.
nbDofTrans()), 0, 3, 1) =
1469 quat_tp.block(1, 0, 3, 1);
1470 QdotOut(Q.size() -
static_cast<unsigned int>(*
m_nRotAQuat + cmpQuat)) =
1478 cmpDof, 0,
static_cast<unsigned int>(segment_i.
nbDof()), 1) =
1480 cmpDof, 0,
static_cast<unsigned int>(segment_i.
nbDof()), 1);
1482 cmpDof +=
static_cast<unsigned int>(segment_i.
nbDof());
1491#ifdef BIORBD_USE_CASADI_MATH
1497 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
1499 return RigidBodyDynamics::Utils::CalcKineticEnergy(
1500 updatedModel, Q, Qdot,
false);
1506#ifdef BIORBD_USE_CASADI_MATH
1513 return RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q,
false);
1520#ifdef BIORBD_USE_CASADI_MATH
1526 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
1528 utils::Scalar kinetic(
1529 RigidBodyDynamics::Utils::CalcKineticEnergy(
1530 updatedModel, Q, Qdot,
false));
1531 utils::Scalar potential(
1532 RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q,
false));
1533 return kinetic - potential;
1540#ifdef BIORBD_USE_CASADI_MATH
1546 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
1548 utils::Scalar kinetic(
1549 RigidBodyDynamics::Utils::CalcKineticEnergy(
1550 updatedModel, Q, Qdot,
false));
1551 utils::Scalar potential(
1552 RigidBodyDynamics::Utils::CalcPotentialEnergy(updatedModel, Q,
false));
1553 return kinetic + potential;
1569#ifdef BIORBD_USE_CASADI_MATH
1579 RigidBodyDynamics::InverseDynamics(updatedModel, Q, Qdot, Qddot, Tau, &fExt);
1594#ifdef BIORBD_USE_CASADI_MATH
1603 RigidBodyDynamics::NonlinearEffects(updatedModel, Q, Qdot, Tau, &fExt);
1620#ifdef BIORBD_USE_CASADI_MATH
1631 RigidBodyDynamics::ForwardDynamics(
1632 updatedModel, Q, Qdot, dampedTau, Qddot, &fExt);
1642 QddotJoints.size() == this->nbQddot() - this->nbRoot(),
1643 "Size of QddotJ must be equal to number of Qddot - "
1644 "number of root coordinates.");
1647 this->
nbRoot() > 0,
"Must have a least one degree of freedom on root.");
1656 static_cast<unsigned int>(this->
nbRoot()),
1657 static_cast<unsigned int>(this->
nbRoot()));
1659 Qddot.block(0, 0,
static_cast<unsigned int>(this->
nbRoot()), 1) =
1662 static_cast<unsigned int>(this->
nbRoot()),
1664 static_cast<unsigned int>(this->
nbQddot() - this->
nbRoot()),
1669#ifdef BIORBD_USE_CASADI_MATH
1671 casadi::Linsol(
"linsol",
"symbolicqr", massMatrixRoot.sparsity());
1672 QRootDDot = linsol.solve(
1674 -MassMatrixNlEffects.block(
1675 0, 0,
static_cast<unsigned int>(this->nbRoot()), 1));
1677 QRootDDot = massMatrixRoot.llt().solve(-MassMatrixNlEffects.block(
1678 0, 0,
static_cast<unsigned int>(this->nbRoot()), 1));
1705 Q, Qdot, Tau, CS, externalForces, updateKin);
1718 Q, Qdot, Tau, CS, forceSet, updateKin);
1729#ifdef BIORBD_USE_CASADI_MATH
1735 updateKin ? &Q :
nullptr, updateKin ? &Qdot :
nullptr);
1741 RigidBodyDynamics::ForwardDynamicsConstraintsDirect(
1742 updatedModel, Q, Qdot, dampedTau, CS, Qddot, updateKin, &fExt);
1754 Q, Qdot, Tau, forceSet);
1777#ifdef BIORBD_USE_CASADI_MATH
1786 RigidBodyDynamics::ComputeConstraintImpulsesDirect(
1787 model, Q, QdotPre, CS, QdotPost);
1795#ifdef BIORBD_USE_CASADI_MATH
1802 for (
size_t i = 1; i < model.mBodies.size(); i++) {
1803 model.Ic[i] = model.I[i];
1806 RigidBodyDynamics::Math::SpatialRigidBodyInertia Itot;
1808 for (
size_t i = model.mBodies.size() - 1; i > 0; i--) {
1809 size_t lambda =
static_cast<size_t>(model.lambda[i]);
1813 model.Ic[lambda] + model.X_lambda[i].applyTranspose(model.Ic[i]);
1815 Itot = Itot + model.X_lambda[i].applyTranspose(model.Ic[i]);
1820 return RigidBodyDynamics::Math::Xtrans(-com)
1821 .applyTranspose(Itot)
1845#ifdef BIORBD_USE_CASADI_MATH
1847 casadi::Linsol(
"linear_solver",
"symbolicqr", body_inertia.sparsity());
1848 RigidBodyDynamics::Math::Vector3d out =
1851 RigidBodyDynamics::Math::Vector3d out =
1868 if (segmentName.compare((*
m_segments)[iB].name())) {
1869 idx += (*m_segments)[iB].nbDof();
1871 idx += (*m_segments)[iB].getDofIdx(dofName);
1883#ifdef BIORBD_USE_CASADI_MATH
1893 if (Q ==
nullptr && Qdot ==
nullptr && Qddot ==
nullptr)
return *
this;
1895#ifdef BIORBD_USE_CASADI_MATH
1900 RigidBodyDynamics::UpdateKinematicsCustom(model, Q, Qdot, Qddot);
1911#ifdef RBDL_ENABLE_LOGGING
1912 LOG <<
"-------- " << __func__ <<
" --------" << std::endl;
1915#ifdef BIORBD_USE_CASADI_MATH
1923 assert(G.rows() == 9 && G.cols() == model.qdot_size);
1925 std::vector<utils::Vector3d> axes;
1929 for (
unsigned int iAxes = 0; iAxes < 3; ++iAxes) {
1931 RigidBodyDynamics::CalcBodyWorldOrientation(
1932 model, Q,
static_cast<unsigned int>(segmentIdx), updateKin)
1936 utils::Matrix3d::Identity(),
1937 bodyMatRot * rotation * *(axes.begin() + iAxes)));
1939 size_t reference_body_id = segmentIdx;
1940 if (model.IsFixedBodyId(
static_cast<unsigned int>(segmentIdx))) {
1941 size_t fbody_id = segmentIdx - model.fixed_body_discriminator;
1942 reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
1944 size_t j = reference_body_id;
1949 unsigned int q_index = model.mJoints[j].q_index;
1951#ifdef BIORBD_USE_CASADI_MATH
1952 if (model.S[j].is_zero() && model.S[j](4).is_zero() &&
1953 model.S[j](5).is_zero())
1955 if (model.S[j](3) != 1.0 && model.S[j](4) != 1.0 && model.S[j](5) != 1.0)
1963 if (model.mJoints[j].mDoFCount == 3) {
1964 G.block(iAxes * 3, q_index, 3, 3) =
1965 ((point_trans * X_base.inverse()).toMatrix() *
1966 model.multdof3_S[j])
1969 G.block(iAxes * 3, q_index, 3, 1) =
1970 point_trans.apply(X_base.inverse().apply(model.S[j]))
1974 j = model.lambda[j];
1981 size_t biorbdSegmentIdx,
1987 utils::Matrix::Zero(9,
static_cast<unsigned int>(
nbQ())));
1989 Q, segmentIdx, utils::Matrix3d::Identity(), jacobianMat, updateKin);
2002 "Wrong size for the Generalized Coordiates, " +
2004 std::to_string(Q->size()));
2008 Qdot->size() ==
nbQdot(),
2009 "Wrong size for the Generalized Velocities, " +
2011 std::to_string(Qdot->size()));
2016 "Wrong size for the Generalized Accelerations, " +
2018 std::to_string(Qddot->size()));
2024 "Wrong size for the Generalized Torques, " +
2026 " got " + std::to_string(torque->size()));
The actual musculoskeletal model that holds everything in biorbd.
utils::Vector getForce() const
Return the force acting on the constraint.
size_t nbContacts() const
Return the number of contacts.
An External force set that can apply forces to the model while computing the dynamics.
std::vector< RigidBodyDynamics::Math::SpatialVector > computeRbdlSpatialVectors(rigidbody::Joints &updatedModel)
The forces in a rbdl compatible format. This won't work if useTranslationalForces or useSoftContacts ...
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
void setGravity(const utils::Vector3d &newGravity)
Set the gravity.
size_t nbQdot() const
Return the number of generalized velocities (Qdot).
std::shared_ptr< std::vector< Segment > > m_segments
All the articulations.
void overrideMeshFolders(const utils::String &folder)
Set the folder for all the meshes.
rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau, bool updateKin=true)
Interface for the forward dynamics with contact of RBDL.
std::vector< utils::Vector3d > CoMddotBySegment(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Return the acceleration of the center of mass of each segment.
utils::Scalar PotentialEnergy(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Interface for the Potential Energy of RBDL.
size_t nbQuat() const
Return the number of segments that are described using quaternions.
int getBodyRbdlIdToBiorbdId(const int idx) const
Return the Biorbd body identification from rbdl.
utils::Vector3d CalcAngularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin)
Calculate the angular momentum of the center of mass.
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,...
std::shared_ptr< size_t > m_nbRoot
The number of DoF on the root segment.
utils::Matrix massMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Get the mass matrix at a given position Q.
std::vector< NodeSegment > CoMbySegment(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment.
std::vector< utils::Vector3d > CoMdotBySegment(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of the center of mass of each segment.
rigidbody::GeneralizedAcceleration ForwardDynamicsFreeFloatingBase(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedAcceleration &QddotJoints)
Biorbd's implementation of forward dynamics with a free floating base.
std::vector< std::vector< size_t > > getDofSubTrees()
Return the rbdl idx of subtrees of each segments.
utils::SpatialVector CalcPointVelocity6D(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity6D. It is pretty much useless in Eigen, but fixes an issue in Casa...
std::vector< Mesh > mesh() const
Return the segment mesh.
utils::Vector3d CoMddot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Return the acceleration of the center of mass.
size_t nbSegment() const
Return the actual number of segment.
size_t AddSegment(const utils::String &segmentName, const utils::String &parentName, const utils::String &translationSequence, const utils::String &rotationSequence, const std::vector< utils::Range > &QRanges, const std::vector< utils::Range > &QdotRanges, const std::vector< utils::Range > &QddotRanges, const std::vector< utils::Scalar > &jointDampings, const SegmentCharacteristics &characteristics, const utils::RotoTrans &referenceFrame)
Add a segment to the model.
void checkGeneralizedDimensions(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr, const GeneralizedTorque *torque=nullptr)
Check for the Generalized coordinates, velocities, acceleration and torque dimensions.
int getBodyRbdlId(const utils::String &segmentName) const
Return the rbdl body identification.
GeneralizedTorque InverseDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot)
Interface for the inverse dynamics of RBDL.
std::shared_ptr< size_t > m_nbQdot
The total number of Qdot.
utils::Matrix projectPointJacobian(const GeneralizedCoordinates &Q, NodeSegment node, bool updateKin)
Return the jacobian matrix of the projected markers for a marker from the model.
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...
void CalcMatRotJacobian(const GeneralizedCoordinates &Q, size_t segmentIdx, const utils::Matrix3d &rotation, utils::Matrix &G, bool updateKin)
Calculate the jacobian matrix of a rotation matrix.
std::vector< Segment > & segments() const
Get all the segments.
void CalcCenterOfMass(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration *Qddot, utils::Scalar &mass, utils::Vector3d &com, utils::Vector3d *comVelocity=NULL, utils::Vector3d *comAcceleration=NULL, utils::Vector3d *angularMomentum=NULL, utils::Vector3d *changeOfAngularMomentum=NULL, bool updateKin=true)
Interface to CalcCenterOfMass. It is pretty much useless in Eigen, but fixes an issue in Casadi where...
utils::Vector3d CoMdot(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of the center of mass.
std::shared_ptr< size_t > m_nbDof
The total number of degrees of freedom.
std::shared_ptr< utils::Scalar > m_totalMass
Mass of all the bodies combined.
size_t nbDof() const
Return the number of degrees of freedom (DoF).
size_t getBodyBiorbdIdToRbdlId(const int idx) const
Return the Rbdl body identification from Biorbd.
std::vector< utils::RotoTrans > localJCS() const
Return all the joint coordinate system (JCS) in its parent reference frame.
std::vector< utils::Vector3d > CalcSegmentsAngularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin)
Calculate the segment's contribution to the whole body angular momentum with respect to the global ce...
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.
GeneralizedVelocity computeQdot(const GeneralizedCoordinates &Q, const GeneralizedCoordinates &Qdot, const utils::Scalar &k_stab=1)
Return the derivate of Q in function of Qdot (if not Quaternion, Qdot is directly returned).
utils::Scalar Lagrangian(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Lagrangian of the system.
utils::Scalar mass() const
Return the total mass of the model.
Joints DeepCopy() const
Deep copy of the joints.
utils::Scalar KineticEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Interface for the Kinetic Energy of RBDL.
std::shared_ptr< size_t > m_nbQ
The total number of Q.
size_t nbGeneralizedTorque() const
Return the number of generalized torque.
Segment & segment(size_t idx)
Get a segment of index idx.
utils::Matrix massMatrixInverse(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Get the inverse mass matrix at a given position Q.
utils::Vector3d CoM(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass.
utils::Vector3d angularMomentum(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Calculate the angular momentum of the center of mass. This method is an interface to CalcAngularMomem...
utils::Vector3d bodyAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Calculate the angular velocity of the model around its center of mass.
NodeSegment projectPoint(const GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, const utils::String &axesToRemove, bool updateKin=true)
Project a point on specific axis of a segment.
std::shared_ptr< bool > m_isKinematicsComputed
If the kinematics are computed.
int getBodyBiorbdId(const utils::String &segmentName) const
Return the biorbd body identification.
utils::Matrix JacobianSegmentRotMat(const rigidbody::GeneralizedCoordinates &Q, size_t segmentIdx, bool updateKin)
Calculate the jacobian matrix of a rotation matrix for a given segment idx.
utils::SpatialTransform CalcBodyWorldTransformation(const GeneralizedCoordinates &Q, const size_t segmentIdx, bool updateKin=true)
Calculate the joint coordinate system (JCS) in global reference frame of a specified segment.
utils::Vector3d getGravity() const
Get the current gravity.
utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for contacts of the forward dynamics with contact of RBDL.
utils::Matrix CoMbySegmentInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the position of the center of mass of each segment in a matrix.
void updateSegmentCharacteristics(size_t idx, const SegmentCharacteristics &characteristics)
updateSegmentCharacteristics Change the inertia characteristics of the segment idx
GeneralizedVelocity ComputeConstraintImpulsesDirect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &QdotPre)
Compute the Qdot post from an impact.
utils::Matrix3d bodyInertia(const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
bodyInertia Return the matrix of inertia of the body expressed in the global reference frame computed...
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 ...
GeneralizedTorque NonLinearEffect(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot)
Interface to NonLinearEffect.
size_t nbQ() const
Return the number of generalized coordinates (Q).
size_t getDofIndex(const utils::String &SegmentName, const utils::String &dofName)
Return the index of a DoF in a segment.
size_t nbQddot() const
Return the number of generalized acceleration (Qddot).
std::shared_ptr< size_t > m_nbQddot
The total number of Qddot.
rigidbody::GeneralizedAcceleration ForwardDynamics(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, const GeneralizedTorque &Tau)
Interface for the forward dynamics of RBDL.
utils::RotoTrans globalJCS(const GeneralizedCoordinates &Q, const utils::String &name, bool updateKin=true)
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q.
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...
std::vector< std::vector< size_t > > recursiveDofSubTrees(std::vector< std::vector< size_t > > subTrees, size_t idx)
Return the rbdl idx of subtrees of each segments.
std::shared_ptr< size_t > m_nRotAQuat
The number of segments per quaternion.
size_t nbRoot() const
Return the dof on the root.
Joints()
Construct a joint model.
virtual ~Joints()
Properly destroy class.
utils::Scalar TotalEnergy(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true)
Compute the Total Energy of System.
utils::Vector3d segmentAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the angular velocity of the segment.
GeneralizedTorque computeDampedTau(const GeneralizedVelocity &Qdot) const
Get the joint dampings to apply to the dynamics.
std::vector< utils::String > nameDof() const
Return the names of the degree of freedom (DoF).
std::vector< utils::Matrix > meshPointsInMatrix(const GeneralizedCoordinates &Q, bool updateKin=true)
Return all the vertices of the mesh points in a matrix.
std::vector< std::vector< utils::Vector3d > > meshPoints(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the vertices of the mesh for all segments in global reference frame.
utils::Vector3d CalcZeroMomentPoint(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true)
Compute the zero moment point on a contact surface.
utils::Matrix CoMJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian matrix of the center of mass.
std::vector< std::vector< MeshFace > > meshFaces() const
Return the mesh faces for all the segments.
Holder for the marker set.
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
std::vector< utils::Matrix > markersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the markers.
size_t nbMarkers() const
Return the number of markers.
A class that holds the geometry of a segment.
A point attached to a segment, generally speaking a skin marker.
bool isAxisKept(size_t i) const
Check if axis is kept.
int nbAxesToRemove() const
Return the number of axes to remove.
Characteristics of a segment, namely the mass, the center of mass, the inertia, the length and its me...
Description of a segment.
bool isRotationAQuaternion() const
Return if the rotation DoF of this segment is a quaternion.
size_t nbDofTrans() const
Return the number of translation DoF of the segment.
size_t nbQddot() const
Return the number of generalized accelerations.
size_t nbQ() const
Return the number of generalized coordinates.
size_t nbQdot() const
Return the number of generalized velocities.
size_t nbDof() const
Return the number of DoF of the segment.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
A wrapper for the Eigen::MatrixXd.
A wrapper for the Eigen::MatrixXd.
const String & parent() const
Return the parent name of the node.
void derivate(const Vector &w)
Return the time derivative of the quaterion.
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).
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.
Wrapper of the Eigen VectorXd.