Biorbd Release_1.13.0
Loading...
Searching...
No Matches
PassiveTorqueExponential.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/PassiveTorques/PassiveTorqueExponential.h"
3
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "Utils/Error.h"
6
7using namespace BIORBD_NAMESPACE;
8
11 : internal_forces::passive_torques::PassiveTorque(),
12 m_k1(std::make_shared<utils::Scalar>(0)),
13 m_k2(std::make_shared<utils::Scalar>(0)),
14 m_b1(std::make_shared<utils::Scalar>(0)),
15 m_b2(std::make_shared<utils::Scalar>(0)),
16 m_qMid(std::make_shared<utils::Scalar>(0)),
17 m_tauEq(std::make_shared<utils::Scalar>(0)),
18 m_pBeta(std::make_shared<utils::Scalar>(0)),
19 m_wMax(std::make_shared<utils::Scalar>(0)),
20 m_sV(std::make_shared<utils::Scalar>(0)),
21 m_deltaP(std::make_shared<utils::Scalar>(0)) {
22 setType();
23}
24
28 : internal_forces::passive_torques::PassiveTorque(other),
29 m_k1(other.m_k1),
30 m_k2(other.m_k2),
31 m_b1(other.m_b1),
32 m_b2(other.m_b2),
33 m_qMid(other.m_qMid),
34 m_tauEq(other.m_tauEq),
35 m_pBeta(other.m_pBeta),
36 m_wMax(other.m_wMax),
37 m_sV(other.m_sV),
38 m_deltaP(other.m_deltaP) {
39 setType();
40}
41
44 const utils::Scalar& k1,
45 const utils::Scalar& k2,
46 const utils::Scalar& b1,
47 const utils::Scalar& b2,
48 const utils::Scalar& qMid,
49 const utils::Scalar& tauEq,
50 const utils::Scalar& pBeta,
51 const utils::Scalar& wMax,
52 const utils::Scalar& sV,
53 const utils::Scalar& deltaP,
54 size_t dofIdx)
55 : internal_forces::passive_torques::PassiveTorque(dofIdx),
56 m_k1(std::make_shared<utils::Scalar>(k1)),
57 m_k2(std::make_shared<utils::Scalar>(k2)),
58 m_b1(std::make_shared<utils::Scalar>(b1)),
59 m_b2(std::make_shared<utils::Scalar>(b2)),
60 m_qMid(std::make_shared<utils::Scalar>(qMid)),
61 m_tauEq(std::make_shared<utils::Scalar>(tauEq)),
62 m_pBeta(std::make_shared<utils::Scalar>(pBeta)),
63 m_wMax(std::make_shared<utils::Scalar>(wMax)),
64 m_sV(std::make_shared<utils::Scalar>(sV)),
65 m_deltaP(std::make_shared<utils::Scalar>(deltaP)) {
66 setType();
67}
68
71 const utils::Scalar& k1,
72 const utils::Scalar& k2,
73 const utils::Scalar& b1,
74 const utils::Scalar& b2,
75 const utils::Scalar& qMid,
76 const utils::Scalar& tauEq,
77 const utils::Scalar& pBeta,
78 const utils::Scalar& wMax,
79 const utils::Scalar& sV,
80 const utils::Scalar& deltaP,
81 size_t dofIdx,
82 const utils::String& jointName)
83 : internal_forces::passive_torques::PassiveTorque(dofIdx, jointName),
84 m_k1(std::make_shared<utils::Scalar>(k1)),
85 m_k2(std::make_shared<utils::Scalar>(k2)),
86 m_b1(std::make_shared<utils::Scalar>(b1)),
87 m_b2(std::make_shared<utils::Scalar>(b2)),
88 m_qMid(std::make_shared<utils::Scalar>(qMid)),
89 m_tauEq(std::make_shared<utils::Scalar>(tauEq)),
90 m_pBeta(std::make_shared<utils::Scalar>(pBeta)),
91 m_wMax(std::make_shared<utils::Scalar>(wMax)),
92 m_sV(std::make_shared<utils::Scalar>(sV)),
93 m_deltaP(std::make_shared<utils::Scalar>(deltaP)) {
94 setType();
95}
96
99
106
110 *m_k1 = *other.m_k1;
111 *m_k2 = *other.m_k2;
112 *m_b1 = *other.m_b1;
113 *m_b2 = *other.m_b2;
114 *m_qMid = *other.m_qMid;
115 *m_tauEq = *other.m_tauEq;
116 *m_pBeta = *other.m_pBeta;
117 *m_wMax = *other.m_wMax;
118 *m_sV = *other.m_sV;
119 *m_deltaP = *other.m_deltaP;
120}
121
122utils::Scalar
125 "passiveTorque for PassiveTorqueExponential must be called with Q");
126}
127
128utils::Scalar
131 const rigidbody::GeneralizedCoordinates& Qdot) const {
132 unsigned int dofIdx(static_cast<unsigned int>(*m_dofIdx));
133 return (*m_b1 * exp(*m_k1 * (Q[dofIdx] - *m_qMid)) +
134 *m_b2 * exp(*m_k2 * (Q[dofIdx] - *m_qMid))) *
135 (1 - *m_pBeta * (Qdot[dofIdx] / (*m_sV * *m_wMax))) *
136 (Q[dofIdx] - *m_deltaP) +
137 *m_tauEq;
138}
139
141 *m_type = internal_forces::passive_torques::TORQUE_TYPE::TORQUE_EXPONENTIAL;
142}
Class PassiveTorqueExponential is a joint passive torque type that linearly evolves passive torque is...
virtual utils::Scalar passiveTorque()
Return the passive torque (not available for this type, Q and QDot must be provided).
std::shared_ptr< utils::Scalar > m_k2
Stiffness of the 2nd exponential.
std::shared_ptr< utils::Scalar > m_b1
Scaling of the 1st exponential.
std::shared_ptr< utils::Scalar > m_b2
Scaling of the 2nd exponential.
PassiveTorqueExponential DeepCopy() const
Deep copy of the linear passive torque.
std::shared_ptr< utils::Scalar > m_k1
Stiffness of the 1st exponential.
void DeepCopy(const PassiveTorque &other)
Deep copy of passive torque.
std::shared_ptr< TORQUE_TYPE > m_type
The type of the passive torque.
std::shared_ptr< size_t > m_dofIdx
Index of the DoF associated with the passive torque.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
Wrapper around the std::string class with augmented functionality.
Definition String.h:18