Biorbd
Release_1.13.0
Toggle main menu visibility
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
12
using namespace
BIORBD_NAMESPACE;
13
14
rigidbody::SoftContactNode::SoftContactNode
()
15
: rigidbody::
NodeSegment
(),
16
m_contactPlane
(
17
std::make_shared<std::pair<utils::
Vector3d
, utils::
Vector3d
>>(
18
utils::
Vector3d
(0, 0, 0),
19
utils::
Vector3d
(0, 0, 1))) {}
20
21
rigidbody::SoftContactNode::SoftContactNode
(
22
const
utils::Scalar &x,
23
const
utils::Scalar &y,
24
const
utils::Scalar &z)
25
: rigidbody::
NodeSegment
(x, y, z),
26
m_contactPlane
(
27
std::make_shared<std::pair<utils::
Vector3d
, utils::
Vector3d
>>(
28
utils::
Vector3d
(0, 0, 0),
29
utils::
Vector3d
(0, 0, 1))) {}
30
31
rigidbody::SoftContactNode::SoftContactNode
(
const
utils::Vector3d
&other)
32
: rigidbody::
NodeSegment
(other),
33
m_contactPlane
(
34
std::make_shared<std::pair<utils::
Vector3d
, utils::
Vector3d
>>(
35
utils::
Vector3d
(0, 0, 0),
36
utils::
Vector3d
(0, 0, 1))) {}
37
38
rigidbody::SoftContactNode::SoftContactNode
(
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),
55
m_contactPlane
(
56
std::make_shared<std::pair<utils::
Vector3d
, utils::
Vector3d
>>(
57
utils::
Vector3d
(0, 0, 0),
58
utils::
Vector3d
(0, 0, 1))) {}
59
60
rigidbody::SoftContactNode::SoftContactNode
(
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),
66
m_contactPlane
(
67
std::make_shared<std::pair<utils::
Vector3d
, utils::
Vector3d
>>(
68
utils::
Vector3d
(0, 0, 0),
69
utils::
Vector3d
(0, 0, 1))) {}
70
71
void
rigidbody::SoftContactNode::DeepCopy
(
72
const
rigidbody::SoftContactNode
&other) {
73
rigidbody::NodeSegment::DeepCopy
(other);
74
*
m_contactPlane
= *other.
m_contactPlane
;
75
}
76
77
utils::SpatialVector
rigidbody::SoftContactNode::computeForceAtOrigin
(
78
Joints
&model,
79
const
GeneralizedCoordinates
&Q,
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());
92
utils::Vector3d
dx(
93
rigidbody::NodeSegment
(
94
updatedModel.CalcPointVelocity(Q, Qdot,
id
, *
this
, updateKin)));
95
96
utils::Vector3d
x(
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
118
void
rigidbody::SoftContactNode::setType
() {
119
*
m_typeOfNode
= utils::NODE_TYPE::SOFT_CONTACT;
120
}
BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates
Class GeneralizedCoordinates.
Definition
GeneralizedCoordinates.h:15
BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity
Class GeneralizedVelocity.
Definition
GeneralizedVelocity.h:15
BIORBD_NAMESPACE::rigidbody::Joints
This is the core of the musculoskeletal model in biorbd.
Definition
Joints.h:48
BIORBD_NAMESPACE::rigidbody::Joints::UpdateKinematicsCustom
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
BIORBD_NAMESPACE::rigidbody::NodeSegment
A point attached to a segment, generally speaking a skin marker.
Definition
NodeSegment.h:20
BIORBD_NAMESPACE::rigidbody::NodeSegment::NodeSegment
NodeSegment()
Construct a segment node.
Definition
NodeSegment.cpp:8
BIORBD_NAMESPACE::rigidbody::NodeSegment::DeepCopy
NodeSegment DeepCopy() const
Deep copy of the segment node.
Definition
NodeSegment.cpp:77
BIORBD_NAMESPACE::rigidbody::SoftContactNode
A point attached to a contact node, generally speaking a skin marker.
Definition
SoftContactNode.h:23
BIORBD_NAMESPACE::rigidbody::SoftContactNode::m_contactPlane
std::shared_ptr< std::pair< utils::Vector3d, utils::Vector3d > > m_contactPlane
Definition
SoftContactNode.h:126
BIORBD_NAMESPACE::rigidbody::SoftContactNode::SoftContactNode
SoftContactNode()
Construct a contact node.
Definition
SoftContactNode.cpp:14
BIORBD_NAMESPACE::rigidbody::SoftContactNode::computeForce
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.
BIORBD_NAMESPACE::rigidbody::SoftContactNode::computeForceAtOrigin
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.
Definition
SoftContactNode.cpp:77
BIORBD_NAMESPACE::rigidbody::SoftContactNode::applicationPoint
virtual utils::Vector3d applicationPoint(const utils::Vector3d &x) const =0
Get the application point relative to the plane.
BIORBD_NAMESPACE::rigidbody::SoftContactNode::setType
void setType()
Set the type of the contact node.
Definition
SoftContactNode.cpp:118
BIORBD_NAMESPACE::utils::Node::name
const String & name() const
Return the name of the node.
Definition
Node.cpp:43
BIORBD_NAMESPACE::utils::Node::m_typeOfNode
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Definition
Node.h:92
BIORBD_NAMESPACE::utils::Node::parent
const String & parent() const
Return the parent name of the node.
Definition
Node.cpp:45
BIORBD_NAMESPACE::utils::SpatialVector
Wrapper of the Eigen::Matrix<double, 6, 1> or Casadi::MX(6, 1).
Definition
SpatialVector.h:21
BIORBD_NAMESPACE::utils::String
Wrapper around the std::string class with augmented functionality.
Definition
String.h:18
BIORBD_NAMESPACE::utils::Vector3d
Wrapper around Eigen Vector3d and attach it to a parent.
Definition
Vector3d.h:27
BIORBD_NAMESPACE::utils::Vector3d::Vector3d
Vector3d()
Construct 3D vector.
Definition
Vector3d.cpp:9
BIORBD_NAMESPACE::utils::Vector3d::DeepCopy
Vector3d DeepCopy() const
Deep copy of a 3D vector.
Definition
Vector3d.cpp:68
src
RigidBody
SoftContactNode.cpp
Generated by
1.18.0