Biorbd Release_1.13.0
Loading...
Searching...
No Matches
SoftContactNode.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/SoftContactNode.h"
3
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "RigidBody/GeneralizedVelocity.h"
6#include "RigidBody/Joints.h"
7#include "RigidBody/Segment.h"
8#include "RigidBody/SegmentCharacteristics.h"
9#include "Utils/Error.h"
10#include "Utils/SpatialVector.h"
11
12using namespace BIORBD_NAMESPACE;
13
15 : rigidbody::NodeSegment(),
17 std::make_shared<std::pair<utils::Vector3d, utils::Vector3d>>(
18 utils::Vector3d(0, 0, 0),
19 utils::Vector3d(0, 0, 1))) {}
20
22 const utils::Scalar &x,
23 const utils::Scalar &y,
24 const utils::Scalar &z)
25 : rigidbody::NodeSegment(x, y, z),
27 std::make_shared<std::pair<utils::Vector3d, utils::Vector3d>>(
28 utils::Vector3d(0, 0, 0),
29 utils::Vector3d(0, 0, 1))) {}
30
32 : rigidbody::NodeSegment(other),
34 std::make_shared<std::pair<utils::Vector3d, utils::Vector3d>>(
35 utils::Vector3d(0, 0, 0),
36 utils::Vector3d(0, 0, 1))) {}
37
39 const utils::Scalar &x,
40 const utils::Scalar &y,
41 const utils::Scalar &z,
42 const utils::String &name,
43 const utils::String &parentName,
44 int parentID)
45 : rigidbody::NodeSegment(
46 x,
47 y,
48 z,
49 name,
50 parentName,
51 true,
52 true,
53 "",
54 parentID),
56 std::make_shared<std::pair<utils::Vector3d, utils::Vector3d>>(
57 utils::Vector3d(0, 0, 0),
58 utils::Vector3d(0, 0, 1))) {}
59
61 const utils::Vector3d &node,
62 const utils::String &name,
63 const utils::String &parentName,
64 int parentID)
65 : rigidbody::NodeSegment(node, name, parentName, true, true, "", parentID),
67 std::make_shared<std::pair<utils::Vector3d, utils::Vector3d>>(
68 utils::Vector3d(0, 0, 0),
69 utils::Vector3d(0, 0, 1))) {}
70
76
78 Joints &model,
80 const GeneralizedVelocity &Qdot,
81 bool updateKin) {
82#ifdef BIORBD_USE_CASADI_MATH
83 Joints
84#else
85 Joints &
86#endif
87 updatedModel = model.UpdateKinematicsCustom(
88 updateKin ? &Q : nullptr, updateKin ? &Qdot : nullptr, nullptr);
89 updateKin = false;
90
91 unsigned int id = updatedModel.GetBodyId(parent().c_str());
94 updatedModel.CalcPointVelocity(Q, Qdot, id, *this, updateKin)));
95
97 updatedModel.CalcBodyToBaseCoordinates(Q, id, *this, updateKin));
98 utils::Vector3d angularVelocity(
99 updatedModel
100 .CalcPointVelocity6D(Q, Qdot, id, utils::Vector3d(0, 0, 0), updateKin)
101 .block(0, 0, 3, 1));
102
103 utils::Vector3d force(computeForce(x, dx, angularVelocity));
104
105 // Transport to CoM (Bour's formula)
106 const utils::Vector3d &CoM(
107 updatedModel.segment(parent()).characteristics().CoM());
108 utils::Vector3d CoMinGlobal(
109 updatedModel.CalcBodyToBaseCoordinates(Q, id, CoM, updateKin));
110
111 // Find the application point of the force
112 utils::SpatialVector out(0., 0., 0., 0., 0., 0.);
113 out.block(0, 0, 3, 1) = force.cross(-applicationPoint(x));
114 out.block(3, 0, 3, 1) = force;
115 return out;
116}
117
119 *m_typeOfNode = utils::NODE_TYPE::SOFT_CONTACT;
120}
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
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 ...
Definition Joints.cpp:1888
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
NodeSegment()
Construct a segment node.
NodeSegment DeepCopy() const
Deep copy of the segment node.
A point attached to a contact node, generally speaking a skin marker.
std::shared_ptr< std::pair< utils::Vector3d, utils::Vector3d > > m_contactPlane
virtual utils::Vector3d computeForce(const utils::Vector3d &x, const utils::Vector3d &dx, const utils::Vector3d &angularVelocity) const =0
Get the force in a spatial vector at the center of mass of the underlying segment.
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.
virtual utils::Vector3d applicationPoint(const utils::Vector3d &x) const =0
Get the application point relative to the plane.
void setType()
Set the type of the contact node.
const String & name() const
Return the name of the node.
Definition Node.cpp:43
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Definition Node.h:92
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Vector3d()
Construct 3D vector.
Definition Vector3d.cpp:9
Vector3d DeepCopy() const
Deep copy of a 3D vector.
Definition Vector3d.cpp:68