Biorbd Release_1.13.0
Loading...
Searching...
No Matches
SoftContactSphere.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/SoftContactSphere.h"
3
4#include "Utils/String.h"
5
6using namespace BIORBD_NAMESPACE;
7
9 : rigidbody::SoftContactNode(),
10 m_radius(std::make_shared<utils::Scalar>(-1)),
11 m_stiffness(std::make_shared<utils::Scalar>(-1)),
12 m_damping(std::make_shared<utils::Scalar>(-1)),
13 m_muStatic(std::make_shared<utils::Scalar>(0.8)),
14 m_muDynamic(std::make_shared<utils::Scalar>(0.7)),
15 m_muViscous(std::make_shared<utils::Scalar>(0.5)),
16 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
17 setType();
18}
19
21 : rigidbody::SoftContactNode(other),
22 m_radius(std::make_shared<utils::Scalar>(-1)),
23 m_stiffness(std::make_shared<utils::Scalar>(-1)),
24 m_damping(std::make_shared<utils::Scalar>(-1)),
25 m_muStatic(std::make_shared<utils::Scalar>(0.8)),
26 m_muDynamic(std::make_shared<utils::Scalar>(0.7)),
27 m_muViscous(std::make_shared<utils::Scalar>(0.5)),
28 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
30 dynamic_cast<const SoftContactSphere &>(other);
31 *m_radius = *tp.m_radius;
33 *m_damping = *tp.m_damping;
34 setType();
35}
36
38 const utils::Scalar &x,
39 const utils::Scalar &y,
40 const utils::Scalar &z,
41 const utils::Scalar &radius,
42 const utils::Scalar &stiffness,
43 const utils::Scalar &damping,
44 const utils::Scalar &m_muStatic,
45 const utils::Scalar &m_muDynamic,
46 const utils::Scalar &m_muViscous)
47 : rigidbody::SoftContactNode(x, y, z),
48 m_radius(std::make_shared<utils::Scalar>(radius)),
49 m_stiffness(std::make_shared<utils::Scalar>(stiffness)),
50 m_damping(std::make_shared<utils::Scalar>(damping)),
51 m_muStatic(std::make_shared<utils::Scalar>(m_muStatic)),
52 m_muDynamic(std::make_shared<utils::Scalar>(m_muDynamic)),
53 m_muViscous(std::make_shared<utils::Scalar>(m_muViscous)),
54 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
55 setType();
56}
57
59 const utils::Vector3d &other,
60 const utils::Scalar &radius,
61 const utils::Scalar &stiffness,
62 const utils::Scalar &damping,
63 const utils::Scalar &m_muStatic,
64 const utils::Scalar &m_muDynamic,
65 const utils::Scalar &m_muViscous)
66 : rigidbody::SoftContactNode(other),
67 m_radius(std::make_shared<utils::Scalar>(radius)),
68 m_stiffness(std::make_shared<utils::Scalar>(stiffness)),
69 m_damping(std::make_shared<utils::Scalar>(damping)),
70 m_muStatic(std::make_shared<utils::Scalar>(m_muStatic)),
71 m_muDynamic(std::make_shared<utils::Scalar>(m_muDynamic)),
72 m_muViscous(std::make_shared<utils::Scalar>(m_muViscous)),
73 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
74 setType();
75}
76
78 const utils::Scalar &x,
79 const utils::Scalar &y,
80 const utils::Scalar &z,
81 const utils::Scalar &radius,
82 const utils::Scalar &stiffness,
83 const utils::Scalar &damping,
84 const utils::Scalar &m_muStatic,
85 const utils::Scalar &m_muDynamic,
86 const utils::Scalar &m_muViscous,
87 const utils::String &name,
88 const utils::String &parentName,
89 int parentID)
90 : rigidbody::SoftContactNode(x, y, z, name, parentName, parentID),
91 m_radius(std::make_shared<utils::Scalar>(radius)),
92 m_stiffness(std::make_shared<utils::Scalar>(stiffness)),
93 m_damping(std::make_shared<utils::Scalar>(damping)),
94 m_muStatic(std::make_shared<utils::Scalar>(m_muStatic)),
95 m_muDynamic(std::make_shared<utils::Scalar>(m_muDynamic)),
96 m_muViscous(std::make_shared<utils::Scalar>(m_muViscous)),
97 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
98 setType();
99}
100
102 const utils::Vector3d &node,
103 const utils::Scalar &radius,
104 const utils::Scalar &stiffness,
105 const utils::Scalar &damping,
106 const utils::Scalar &m_muStatic,
107 const utils::Scalar &m_muDynamic,
108 const utils::Scalar &m_muViscous,
109 const utils::String &name,
110 const utils::String &parentName,
111 int parentID)
112 : rigidbody::SoftContactNode(node, name, parentName, parentID),
113 m_radius(std::make_shared<utils::Scalar>(radius)),
114 m_stiffness(std::make_shared<utils::Scalar>(stiffness)),
115 m_damping(std::make_shared<utils::Scalar>(damping)),
116 m_muStatic(std::make_shared<utils::Scalar>(m_muStatic)),
117 m_muDynamic(std::make_shared<utils::Scalar>(m_muDynamic)),
118 m_muViscous(std::make_shared<utils::Scalar>(m_muViscous)),
119 m_transitionVelocity(std::make_shared<utils::Scalar>(0.01)) {
120 setType();
121}
122
128
139
141 *m_radius = radius;
142}
143
144utils::Scalar rigidbody::SoftContactSphere::radius() const { return *m_radius; }
145
147 const utils::Scalar &stiffness) {
149}
150
152 return *m_stiffness;
153}
154
157}
158
160 return *m_damping;
161}
162
166
168 return *m_muStatic;
169}
170
172 const utils::Scalar &muDynamic) {
174}
175
177 return *m_muDynamic;
178}
179
181 const utils::Scalar &muViscous) {
183}
184
186 return *m_muViscous;
187}
188
193
197
199 const utils::Vector3d &x,
200 const utils::Vector3d &dx,
201 const utils::Vector3d &angularVelocity) const {
202 // Indentation detection with the ground
203 const utils::Vector3d &plane(m_contactPlane->first);
204 const utils::Vector3d &normal(m_contactPlane->second);
205
206 // Decomposition into normal and tangent velocities
207 utils::Scalar normalVelocity = dx.dot(normal);
208 utils::Vector3d tangentVelocity = dx - normalVelocity * normal +
209 (*m_radius * normal).cross(angularVelocity);
210
211 // Penetration of the sphere in the plane
212 utils::Scalar delta = -((x - plane).dot(normal) - *m_radius);
213 utils::Scalar deltaDot = -normalVelocity;
214
215 // Compute the smoothing factor
216 utils::Scalar eps(1e-16);
217 utils::Scalar bv(50);
218 utils::Scalar bd(300);
219 utils::Scalar fslope =
220 (0.5 + 0.5 * std::tanh(bd * delta) + eps) *
221 (0.5 + 0.5 * std::tanh(bv * (deltaDot + 2. / 3. / *m_damping) + eps));
222
223 // Force factor on normal from Hertz's model
224 // sqrt and **2 to get positive values in case of negative penetration
225 utils::Scalar deltaSquaredRooted(std::sqrt(delta * delta));
226 utils::Scalar forceFactor =
227 4. / 3. * *m_stiffness * std::sqrt(*m_radius) *
228 std::sqrt(deltaSquaredRooted * deltaSquaredRooted * deltaSquaredRooted);
229
230 // Hunt-Crossley' model
231 utils::Scalar fHC = forceFactor * (1. + 1.5 * *m_damping * deltaDot);
232 utils::Scalar normalForce = fHC * fslope;
233
234 utils::Scalar tangentVelocityNorm(
235 std::sqrt(tangentVelocity.squaredNorm() + 1e-5));
236 utils::Scalar frictionVelocity = tangentVelocityNorm / *m_transitionVelocity;
237
238 // from Peter Brown 2017
239 utils::Scalar forceFriction =
240 normalForce * *m_muDynamic * std::tanh(4. * frictionVelocity) +
241 normalForce * (*m_muStatic - *m_muDynamic) * frictionVelocity /
242 ((0.25 * frictionVelocity * frictionVelocity + 0.75) *
243 (0.25 * frictionVelocity * frictionVelocity + 0.75)) +
244 normalForce * *m_muViscous * tangentVelocityNorm;
245
246 // Total Force
247 return normalForce * normal +
248 forceFriction * -tangentVelocity / tangentVelocityNorm;
249}
250
252 const utils::Vector3d &x) const {
253 const utils::Vector3d &plane(m_contactPlane->first);
254 const utils::Vector3d &normal(m_contactPlane->second);
255 utils::Scalar delta = -((x - plane).dot(normal) - *m_radius);
256
257 return (x - plane) - delta * normal;
258}
259
261 *m_typeOfNode = utils::NODE_TYPE::SOFT_CONTACT_SPHERE;
262}
std::shared_ptr< std::pair< utils::Vector3d, utils::Vector3d > > m_contactPlane
A point attached to a contact node, generally speaking a skin marker.
utils::Scalar damping() const
Return the value of the damping.
std::shared_ptr< utils::Scalar > m_damping
The damping factor of the contact sphere.
SoftContactSphere DeepCopy() const
Deep copy of the contact node.
utils::Scalar muStatic() const
Return the value of the muStatic.
utils::Scalar radius() const
Return the value of the radius.
void setTransitionVelocity(const utils::Scalar &transitionVelocity)
Set a new value for the transitionVelocity.
virtual utils::Vector3d applicationPoint(const utils::Vector3d &x) const
Get the application point relative to the plane.
std::shared_ptr< utils::Scalar > m_muViscous
Vicous coefficient of friction (mu).
std::shared_ptr< utils::Scalar > m_transitionVelocity
std::shared_ptr< utils::Scalar > m_muStatic
Static coefficient of friction (mu).
utils::Scalar muViscous() const
Return the value of the muViscous.
void setType()
Set the type of the contact node.
void setDamping(const utils::Scalar &damping)
Set a new value for the damping.
utils::Scalar transitionVelocity() const
Return the value of the transitionVelocity.
std::shared_ptr< utils::Scalar > m_radius
The radius of the contact sphere.
std::shared_ptr< utils::Scalar > m_muDynamic
Dynamic coefficient of friction (mu).
void setMuStatic(const utils::Scalar &muStatic)
Set a new value for the muStatic.
void setMuDynamic(const utils::Scalar &muDynamic)
Set a new value for the muDynamic.
void setRadius(const utils::Scalar &radius)
Set a new value for the radius.
utils::Scalar muDynamic() const
Return the value of the muDynamic.
virtual utils::Vector3d computeForce(const utils::Vector3d &x, const utils::Vector3d &dx, const utils::Vector3d &angularVelocity) const
Get the force in a spatial vector at the center of mass of the underlying segment.
void setStiffness(const utils::Scalar &stiffness)
Set a new value for the stiffness.
utils::Scalar stiffness() const
Return the value of the stiffness.
std::shared_ptr< utils::Scalar > m_stiffness
The stiffness of the contact sphere.
void setMuViscous(const utils::Scalar &muViscous)
Set a new value for the muViscous.
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
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 DeepCopy() const
Deep copy of a 3D vector.
Definition Vector3d.cpp:68