summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp')
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp345
1 files changed, 216 insertions, 129 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
index becfe0f4a2..f150b5ae4c 100644
--- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
@@ -8,69 +8,84 @@
#include "details/MultiBodyTreeImpl.hpp"
#include "details/MultiBodyTreeInitCache.hpp"
-namespace btInverseDynamics {
-
+namespace btInverseDynamics
+{
MultiBodyTree::MultiBodyTree()
: m_is_finalized(false),
m_mass_parameters_are_valid(true),
m_accept_invalid_mass_parameters(false),
m_impl(0x0),
- m_init_cache(0x0) {
+ m_init_cache(0x0)
+{
m_init_cache = new InitCache();
}
-MultiBodyTree::~MultiBodyTree() {
+MultiBodyTree::~MultiBodyTree()
+{
delete m_impl;
delete m_init_cache;
}
-void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
+void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
+{
m_accept_invalid_mass_parameters = flag;
}
-bool MultiBodyTree::getAcceptInvalidMassProperties() const {
+bool MultiBodyTree::getAcceptInvalidMassProperties() const
+{
return m_accept_invalid_mass_parameters;
}
-int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
+int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
+{
return m_impl->getBodyOrigin(body_index, world_origin);
}
-int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
+int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
+{
return m_impl->getBodyCoM(body_index, world_com);
}
-int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
+int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
+{
return m_impl->getBodyTransform(body_index, world_T_body);
}
-int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
+int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
+{
return m_impl->getBodyAngularVelocity(body_index, world_omega);
}
-int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
+int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
+{
return m_impl->getBodyLinearVelocity(body_index, world_velocity);
}
-int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
+int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
+{
return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
}
-int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
+int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
+{
return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
}
-int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
+int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
+{
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
}
-int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
- return m_impl->getParentRParentBodyRef(body_index, r);
+int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
+{
+ return m_impl->getParentRParentBodyRef(body_index, r);
}
-int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
- return m_impl->getBodyTParentRef(body_index, T);
+int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
+{
+ return m_impl->getBodyTParentRef(body_index, T);
}
-int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
- return m_impl->getBodyAxisOfMotion(body_index, axis);
+int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
+{
+ return m_impl->getBodyAxisOfMotion(body_index, axis);
}
void MultiBodyTree::printTree() { m_impl->printTree(); }
@@ -81,12 +96,15 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
- vecx *joint_forces) {
- if (false == m_is_finalized) {
+ vecx *joint_forces)
+{
+ if (false == m_is_finalized)
+ {
bt_id_error_message("system has not been initialized\n");
return -1;
}
- if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
+ if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
+ {
bt_id_error_message("error in inverse dynamics calculation\n");
return -1;
}
@@ -95,141 +113,164 @@ int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const
int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
const bool initialize_matrix,
- const bool set_lower_triangular_matrix, matxx *mass_matrix) {
- if (false == m_is_finalized) {
+ const bool set_lower_triangular_matrix, matxx *mass_matrix)
+{
+ if (false == m_is_finalized)
+ {
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 ==
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
- set_lower_triangular_matrix, mass_matrix)) {
+ set_lower_triangular_matrix, mass_matrix))
+ {
bt_id_error_message("error in mass matrix calculation\n");
return -1;
}
return 0;
}
-int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
+int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
+{
return calculateMassMatrix(q, true, true, true, mass_matrix);
}
+int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
+{
+ vec3 world_gravity(m_impl->m_world_gravity);
+ // temporarily set gravity to zero, to ensure we get the actual accelerations
+ setZero(m_impl->m_world_gravity);
+ if (false == m_is_finalized)
+ {
+ bt_id_error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateKinematics(q, u, dot_u,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION))
+ {
+ bt_id_error_message("error in kinematics calculation\n");
+ return -1;
+ }
-int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
- vec3 world_gravity(m_impl->m_world_gravity);
- // temporarily set gravity to zero, to ensure we get the actual accelerations
- setZero(m_impl->m_world_gravity);
-
- if (false == m_is_finalized) {
- bt_id_error_message("system has not been initialized\n");
- return -1;
- }
- if (-1 == m_impl->calculateKinematics(q, u, dot_u,
- MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
- bt_id_error_message("error in kinematics calculation\n");
- return -1;
- }
-
- m_impl->m_world_gravity=world_gravity;
- return 0;
+ m_impl->m_world_gravity = world_gravity;
+ return 0;
}
-
-int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
- if (false == m_is_finalized) {
+int MultiBodyTree::calculatePositionKinematics(const vecx &q)
+{
+ if (false == m_is_finalized)
+ {
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, q, q,
- MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
+ {
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
-int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
- if (false == m_is_finalized) {
+int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
+{
+ if (false == m_is_finalized)
+ {
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, u,
- MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
+ {
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
-
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
- if (false == m_is_finalized) {
- bt_id_error_message("system has not been initialized\n");
- return -1;
- }
- if (-1 == m_impl->calculateJacobians(q, u,
- MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
- bt_id_error_message("error in jacobian calculation\n");
- return -1;
- }
- return 0;
+int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
+{
+ if (false == m_is_finalized)
+ {
+ bt_id_error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateJacobians(q, u,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
+ {
+ bt_id_error_message("error in jacobian calculation\n");
+ return -1;
+ }
+ return 0;
}
-int MultiBodyTree::calculateJacobians(const vecx& q){
- if (false == m_is_finalized) {
- bt_id_error_message("system has not been initialized\n");
- return -1;
- }
- if (-1 == m_impl->calculateJacobians(q, q,
- MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
- bt_id_error_message("error in jacobian calculation\n");
- return -1;
- }
- return 0;
+int MultiBodyTree::calculateJacobians(const vecx &q)
+{
+ if (false == m_is_finalized)
+ {
+ bt_id_error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateJacobians(q, q,
+ MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
+ {
+ bt_id_error_message("error in jacobian calculation\n");
+ return -1;
+ }
+ return 0;
}
-int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
- return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
+int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
+{
+ return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
}
-int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
- return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
+int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
+{
+ return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
}
-int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
- return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
+int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
+{
+ return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
}
-int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const {
- return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
+int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
+{
+ return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
}
-
#endif
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
const vec3 &body_axis_of_motion_, idScalar mass,
const vec3 &body_r_body_com, const mat33 &body_I_body,
- const int user_int, void *user_ptr) {
- if (body_index < 0) {
+ const int user_int, void *user_ptr)
+{
+ if (body_index < 0)
+ {
bt_id_error_message("body index must be positive (got %d)\n", body_index);
return -1;
}
vec3 body_axis_of_motion(body_axis_of_motion_);
- switch (joint_type) {
+ switch (joint_type)
+ {
case REVOLUTE:
case PRISMATIC:
// check if axis is unit vector
- if (!isUnitVector(body_axis_of_motion)) {
+ if (!isUnitVector(body_axis_of_motion))
+ {
bt_id_warning_message(
"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
- BT_ID_POW(body_axis_of_motion(1), 2) +
- BT_ID_POW(body_axis_of_motion(2), 2));
- if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) {
+ BT_ID_POW(body_axis_of_motion(1), 2) +
+ BT_ID_POW(body_axis_of_motion(2), 2));
+ if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min()))
+ {
bt_id_error_message("axis of motion vector too short (%e)\n", length);
return -1;
}
@@ -240,29 +281,36 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
break;
case FLOATING:
break;
+ case SPHERICAL:
+ break;
default:
bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
}
// sanity check for mass properties. Zero mass is OK.
- if (mass < 0) {
+ if (mass < 0)
+ {
m_mass_parameters_are_valid = false;
bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
- if (!m_accept_invalid_mass_parameters) {
+ if (!m_accept_invalid_mass_parameters)
+ {
return -1;
}
}
- if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
+ if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
+ {
m_mass_parameters_are_valid = false;
// error message printed in function call
- if (!m_accept_invalid_mass_parameters) {
+ if (!m_accept_invalid_mass_parameters)
+ {
return -1;
}
}
- if (!isValidTransformMatrix(body_T_parent_ref)) {
+ if (!isValidTransformMatrix(body_T_parent_ref))
+ {
return -1;
}
@@ -271,52 +319,63 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
body_I_body, user_int, user_ptr);
}
-int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
+int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
+{
return m_impl->getParentIndex(body_index, parent_index);
}
-int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
+int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
+{
return m_impl->getUserInt(body_index, user_int);
}
-int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
+int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
+{
return m_impl->getUserPtr(body_index, user_ptr);
}
-int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
+int MultiBodyTree::setUserInt(const int body_index, const int user_int)
+{
return m_impl->setUserInt(body_index, user_int);
}
-int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
+int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
+{
return m_impl->setUserPtr(body_index, user_ptr);
}
-int MultiBodyTree::finalize() {
+int MultiBodyTree::finalize()
+{
const int &num_bodies = m_init_cache->numBodies();
const int &num_dofs = m_init_cache->numDoFs();
- if(num_dofs<=0) {
- bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
- //return -1;
- }
+ if (num_dofs <= 0)
+ {
+ bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
+ //return -1;
+ }
// 1 allocate internal MultiBody structure
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
// 2 build new index set assuring index(parent) < index(child)
- if (-1 == m_init_cache->buildIndexSets()) {
+ if (-1 == m_init_cache->buildIndexSets())
+ {
return -1;
}
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
// 3 setup internal kinematic and dynamic data
- for (int index = 0; index < num_bodies; index++) {
+ for (int index = 0; index < num_bodies; index++)
+ {
InertiaData inertia;
JointData joint;
- if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
+ if (-1 == m_init_cache->getInertiaData(index, &inertia))
+ {
return -1;
}
- if (-1 == m_init_cache->getJointData(index, &joint)) {
+ if (-1 == m_init_cache->getJointData(index, &joint))
+ {
return -1;
}
@@ -332,24 +391,29 @@ int MultiBodyTree::finalize() {
rigid_body.m_joint_type = joint.m_type;
int user_int;
- if (-1 == m_init_cache->getUserInt(index, &user_int)) {
+ if (-1 == m_init_cache->getUserInt(index, &user_int))
+ {
return -1;
}
- if (-1 == m_impl->setUserInt(index, user_int)) {
+ if (-1 == m_impl->setUserInt(index, user_int))
+ {
return -1;
}
- void* user_ptr;
- if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
+ void *user_ptr;
+ if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
+ {
return -1;
}
- if (-1 == m_impl->setUserPtr(index, user_ptr)) {
+ if (-1 == m_impl->setUserPtr(index, user_ptr))
+ {
return -1;
}
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
// matrices.
- switch (rigid_body.m_joint_type) {
+ switch (rigid_body.m_joint_type)
+ {
case REVOLUTE:
rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
@@ -375,6 +439,16 @@ int MultiBodyTree::finalize() {
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
+ case SPHERICAL:
+ // NOTE/TODO: this is not really correct.
+ // the Jacobians should be 3x3 matrices here !
+ rigid_body.m_Jac_JR(0) = 0.0;
+ rigid_body.m_Jac_JR(1) = 0.0;
+ rigid_body.m_Jac_JR(2) = 0.0;
+ rigid_body.m_Jac_JT(0) = 0.0;
+ rigid_body.m_Jac_JT(1) = 0.0;
+ rigid_body.m_Jac_JT(2) = 0.0;
+ break;
case FLOATING:
// NOTE/TODO: this is not really correct.
// the Jacobians should be 3x3 matrices here !
@@ -392,7 +466,8 @@ int MultiBodyTree::finalize() {
}
// 4 assign degree of freedom indices & build per-joint-type index arrays
- if (-1 == m_impl->generateIndexSets()) {
+ if (-1 == m_impl->generateIndexSets())
+ {
bt_id_error_message("generating index sets\n");
return -1;
}
@@ -408,54 +483,66 @@ int MultiBodyTree::finalize() {
return 0;
}
-int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
+int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
+{
return m_impl->setGravityInWorldFrame(gravity);
}
-int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
+int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
+{
return m_impl->getJointType(body_index, joint_type);
}
-int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
+int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
+{
return m_impl->getJointTypeStr(body_index, joint_type);
}
-int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
+int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
+{
return m_impl->getDoFOffset(body_index, q_offset);
}
-int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
+int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
+{
return m_impl->setBodyMass(body_index, mass);
}
-int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
+int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
+{
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
}
-int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
+int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
+{
return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
}
-int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
+int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
+{
return m_impl->getBodyMass(body_index, mass);
}
-int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
+int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
+{
return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
}
-int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
+int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
+{
return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
}
void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
-int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
+int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
+{
return m_impl->addUserForce(body_index, body_force);
}
-int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
+int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
+{
return m_impl->addUserMoment(body_index, body_moment);
}
-}
+} // namespace btInverseDynamics
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-17 00:58:27 +0000