1#define BIORBD_API_EXPORTS
3#include "InternalForces/Geometry.h"
7#include <rbdl/Kinematics.h>
9#include "InternalForces/PathModifiers.h"
10#include "InternalForces/ViaPoint.h"
11#include "InternalForces/WrappingObject.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedVelocity.h"
14#include "RigidBody/Joints.h"
15#include "RigidBody/NodeSegment.h"
16#include "Utils/Error.h"
17#include "Utils/Matrix.h"
18#include "Utils/RotoTrans.h"
20using namespace BIORBD_NAMESPACE;
23 :
m_origin(std::make_shared<utils::Vector3d>()),
26 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
28 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
33 m_length(std::make_shared<utils::Scalar>(0)),
34 m_velocity(std::make_shared<utils::Scalar>(0)),
42 :
m_origin(std::make_shared<utils::Vector3d>(origin)),
43 m_insertion(std::make_shared<utils::Vector3d>(insertion)),
45 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
47 std::make_shared<utils::Vector3d>(utils::Vector3d::Zero())),
52 m_length(std::make_shared<utils::Scalar>(0)),
53 m_velocity(std::make_shared<utils::Scalar>(0)),
142 m_origin->RigidBodyDynamics::Math::Vector3d::operator=(position);
155 m_insertion->RigidBodyDynamics::Math::Vector3d::operator=(position);
166 "Geometry must be computed at least once before calling originInLocal()");
172 "Geometry must be computed at least once before calling "
173 "insertionInGlobal()");
180 "Geometry must be computed at least once before calling "
181 "musclesPointsInGlobal()");
189 "Geometry must be computed at least before calling length()");
196 "Geometry must be computed before calling velocity()");
204 "Geometry must be computed before calling jacobian()");
210 "Geometry must be computed before calling jacobianOrigin()");
216 "Geometry must be computed before calling jacobianInsertion()");
222 "Geometry must be computed before calling jacobian(i)");
224 3 *
static_cast<unsigned int>(idxViaPoint), 0, 3,
m_jacobian->cols());
230 "Geometry must be computed before calling jacobianLength()");
245 if (Qdot !=
nullptr) {
272 std::vector<utils::Vector3d> &ptsInGlobal) {
274 ptsInGlobal.size() >= 2,
275 "ptsInGlobal must at least have an origin and an insertion");
291 if (pathModifiers->
nbWraps() != 0) {
294 pathModifiers->
nbVia() == 0,
"Cannot mix wrapping and via points yet");
297 "Cannot compute more than one wrapping yet");
302 pathModifiers->
object(0));
320 w.
wrapPoints(RT, po_mus, pi_mus, po_wrap, pi_wrap, &a);
349 for (
size_t i = 0; i < pathModifiers->
nbObjects(); ++i) {
359 }
else if (pathModifiers->
nbObjects() == 0) {
378 if (pathModifiers !=
nullptr && pathModifiers->
nbWraps() != 0) {
380 pathModifiers->
nbVia() == 0,
"Cannot mix wrapping and via points yet");
383 "Cannot compute more than one wrapping yet");
393 utils::Scalar lengthWrap(0);
397 ((*m_pointsInGlobal)[0] - po_wrap).norm() +
399 ((*m_pointsInGlobal)[3] - pi_wrap).norm();
403 *
m_length += ((*m_pointsInGlobal)[i + 1] - (*m_pointsInGlobal)[i]).norm();
418 static_cast<unsigned int>(
m_pointsInLocal->size() * 3), model.dof_count);
424 "Jacobian is the wrong size");
433 bool updateKin =
false;
435 m_jacobian->block(3 *
static_cast<unsigned int>(i), 0, 3, model.dof_count) =
449 for (
size_t i = 0; i < p.size() - 1; ++i) {
452 (p[i + 1] - p[i]).norm();
Class Geometry of the muscle.
std::shared_ptr< utils::Matrix > m_jacobianLength
The muscle length jacobian.
std::shared_ptr< utils::Vector3d > m_insertion
Insertion node.
utils::Matrix jacobianOrigin() const
Return the gradient of the jacobian for the origin node.
Geometry()
Construct geometry.
const utils::Vector3d & insertionInGlobal() const
Return the insertion position in the global reference.
utils::Matrix jacobianInsertion() const
Return the gradient of the jacobian for the insertion node.
const utils::Scalar & velocity() const
Return the previously computed velocity.
std::shared_ptr< bool > m_isGeometryComputed
std::shared_ptr< utils::Matrix > m_jacobian
The jacobian matrix.
std::shared_ptr< utils::Vector3d > m_originInGlobal
Position of the origin in the global reference.
void computeJacobianLength()
Compute the muscle length jacobian.
void setPointsInGlobal(std::vector< utils::Vector3d > &ptsInGlobal)
Set the muscle points in global.
std::shared_ptr< bool > m_posAndJacoWereForced
const utils::Vector3d & insertionInLocal() const
Return the insertion position.
Geometry DeepCopy() const
Deep copy of a geometry.
std::shared_ptr< utils::Scalar > m_length
length
const std::vector< utils::Vector3d > & pointsInGlobal() const
Return the muscles points in the global reference.
void setInsertionInLocal(const utils::Vector3d &position)
Set the insertion position.
std::shared_ptr< utils::Scalar > m_velocity
Velocity of the muscular elongation.
std::shared_ptr< utils::Vector3d > m_insertionInGlobal
void setJacobianDimension(rigidbody::Joints &model)
Set the jacobian dimensions.
const utils::Matrix & jacobian() const
Return the previously computed muscle jacobian.
void _updateKinematics(const rigidbody::GeneralizedVelocity *Qdot, internal_forces::PathModifiers *pathModifiers=nullptr)
Actual function that implements the update of the kinematics.
std::shared_ptr< std::vector< utils::Vector3d > > m_pointsInLocal
Position of all the points in local.
const utils::Vector3d & originInGlobal() const
Return the origin position in the global reference.
const utils::Matrix & jacobianLength() const
Return the jacobian length of the muscle.
void updateKinematics(rigidbody::Joints &updatedModel, const rigidbody::GeneralizedCoordinates *Q=nullptr, const rigidbody::GeneralizedVelocity *Qdot=nullptr)
Updates the position and dynamic elements of the muscles.
void setOrigin(const utils::Vector3d &position)
Set the origin position in the local reference frame of the muscle.
std::shared_ptr< bool > m_isVelocityComputed
std::shared_ptr< utils::Vector3d > m_origin
Origin node.
const utils::Scalar & length() const
Return the previously computed muscle length.
const utils::Vector3d & originInLocal() const
Return the origin position.
std::shared_ptr< std::vector< utils::Vector3d > > m_pointsInGlobal
Position of all the points in the global reference.
Holder of all the path modifiers of a muscle.
size_t nbVia() const
Return the total number of via points in the set.
size_t nbWraps() const
Return the total number of wrapping objects in the set.
size_t nbObjects() const
Return the total number of path modifier objects in the set.
utils::Vector3d & object(size_t idx)
Return the object at a specific index in the set.
Base class for the wrapping objects.
virtual const utils::RotoTrans & RT(rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)=0
Return the RotoTrans matrix of the wrapping object.
virtual void wrapPoints(const utils::RotoTrans &rt, const utils::Vector3d &p1_bone, const utils::Vector3d &p2_bone, utils::Vector3d &p1, utils::Vector3d &p2, utils::Scalar *muscleLength=nullptr)=0
From the position of the wrapping object, return the 2 locations where the muscle leaves the wrapping...
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
This is the core of the musculoskeletal model in biorbd.
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 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.
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.
NODE_TYPE typeOfNode() const
Return the type of node.
const String & parent() const
Return the parent name of the node.
Homogenous matrix to describe translations and rotations simultaneously.
Wrapper around Eigen Vector3d and attach it to a parent.