Skip to content

Commit a759c9a

Browse files
ahcordeahcorde
authored
Added Friction and ODE classes (#955)
Signed-off-by: Alejandro Hernández <ahcorde@gmail.com>
1 parent 0fa9e78 commit a759c9a

File tree

5 files changed

+653
-0
lines changed

5 files changed

+653
-0
lines changed

‎include/sdf/Surface.hh

+106
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,103 @@ namespace sdf
5858
IGN_UTILS_IMPL_PTR(dataPtr)
5959
};
6060

61+
/// \brief ODE information for a friction.
62+
class SDFORMAT_VISIBLE ODE
63+
{
64+
/// \brief Default constructor
65+
public: ODE();
66+
67+
/// \brief Load the ODE based on a element pointer. This is *not* the
68+
/// usual entry point. Typical usage of the SDF DOM is through the Root
69+
/// object.
70+
/// \param[in] _sdf The SDF Element pointer
71+
/// \return Errors, which is a vector of Error objects. Each Error includes
72+
/// an error code and message. An empty vector indicates no error.
73+
public: Errors Load(ElementPtr _sdf);
74+
75+
/// \brief Set the Mu
76+
/// \returns ODE mu
77+
public: double Mu() const;
78+
79+
/// \brief Set Mu
80+
/// \param[in] _mu ODE mu
81+
public: void SetMu(double _mu);
82+
83+
/// \brief Get the Mu2
84+
/// \returns ODE mu2
85+
public: double Mu2() const;
86+
87+
/// \brief Set Mu2
88+
/// \param[in] _mu2 ODE mu2
89+
public: void SetMu2(double _mu2);
90+
91+
/// \brief Get the fdir
92+
/// \returns ODE fdir
93+
public: const ignition::math::Vector3d &Fdir1() const;
94+
95+
/// \brief Set fdir
96+
/// \param[in] _fdir ODE fdir
97+
public: void SetFdir1(const ignition::math::Vector3d &_fdir);
98+
99+
/// \brief Get the slip1
100+
/// \returns ODE slip1
101+
public: double Slip1() const;
102+
103+
/// \brief Set Slip1
104+
/// \param[in] _slip1 ODE Slip1
105+
public: void SetSlip1(double _slip1);
106+
107+
/// \brief Get the Slip2
108+
/// \returns ODE Slip2
109+
public: double Slip2() const;
110+
111+
/// \brief Set Slip2
112+
/// \param[in] _slip2 ODE Slip2
113+
public: void SetSlip2(double _slip2);
114+
115+
/// \brief Get a pointer to the SDF element that was used during
116+
/// load.
117+
/// \return SDF element pointer. The value will be nullptr if Load has
118+
/// not been called.
119+
public: sdf::ElementPtr Element() const;
120+
121+
/// \brief Private data pointer.
122+
IGN_UTILS_IMPL_PTR(dataPtr)
123+
};
124+
125+
/// \brief Friction information for a surface.
126+
class SDFORMAT_VISIBLE Friction
127+
{
128+
/// \brief Default constructor
129+
public: Friction();
130+
131+
/// \brief Load the friction based on a element pointer. This is *not* the
132+
/// usual entry point. Typical usage of the SDF DOM is through the Root
133+
/// object.
134+
/// \param[in] _sdf The SDF Element pointer
135+
/// \return Errors, which is a vector of Error objects. Each Error includes
136+
/// an error code and message. An empty vector indicates no error.
137+
public: Errors Load(ElementPtr _sdf);
138+
139+
/// \brief Get the associated ODE object
140+
/// \returns Pointer to the associated ODE object,
141+
/// nullptr if the Surface doesn't contain a ODE element.
142+
public: const sdf::ODE *ODE() const;
143+
144+
/// \brief Set the associated ODE object.
145+
/// \param[in] _ode The ODE object.
146+
public: void SetODE(const sdf::ODE &_ode);
147+
148+
/// \brief Get a pointer to the SDF element that was used during
149+
/// load.
150+
/// \return SDF element pointer. The value will be nullptr if Load has
151+
/// not been called.
152+
public: sdf::ElementPtr Element() const;
153+
154+
/// \brief Private data pointer.
155+
IGN_UTILS_IMPL_PTR(dataPtr)
156+
};
157+
61158
/// \brief Surface information for a collision.
62159
class SDFORMAT_VISIBLE Surface
63160
{
@@ -87,6 +184,15 @@ namespace sdf
87184
/// \param[in] _cont The contact object.
88185
public: void SetContact(const sdf::Contact &_contact);
89186

187+
/// \brief Get the associated friction object
188+
/// \returns Pointer to the associated friction object,
189+
/// nullptr if the Surface doesn't contain a friction element.
190+
public: const sdf::Friction *Friction() const;
191+
192+
/// \brief Set the associated friction object.
193+
/// \param[in] _friction The friction object.
194+
public: void SetFriction(const sdf::Friction &_friction);
195+
90196
/// \brief Create and return an SDF element filled with data from this
91197
/// surface.
92198
/// Note that parameter passing functionality is not captured with this

‎src/Surface.cc

+229
Original file line numberDiff line numberDiff line change
@@ -33,15 +33,225 @@ class sdf::Contact::Implementation
3333
public: sdf::ElementPtr sdf{nullptr};
3434
};
3535

36+
class sdf::ODE::Implementation
37+
{
38+
/// \brief The SDF element pointer used during load.
39+
public: sdf::ElementPtr sdf{nullptr};
40+
41+
/// \brief Coefficient of friction in first friction pyramid direction,
42+
/// the unitless maximum ratio of force in first friction pyramid
43+
/// direction to normal force.
44+
public: double mu = 1.0;
45+
46+
/// \brief Coefficient of friction in second friction pyramid direction,
47+
/// the unitless maximum ratio of force in second friction pyramid
48+
/// direction to normal force.
49+
public: double mu2 = 1.0;
50+
51+
/// \brief Unit vector specifying first friction pyramid direction in
52+
/// collision-fixed reference frame.
53+
public: ignition::math::Vector3d fdir1 = {0, 0, 0};
54+
55+
/// \brief Force dependent slip in first friction pyramid direction,
56+
/// equivalent to inverse of viscous damping coefficient
57+
/// with units of m/s/N.
58+
public: double slip1 = 0.0;
59+
60+
/// \brief Force dependent slip in second friction pyramid direction,
61+
/// equivalent to inverse of viscous damping coefficient
62+
/// with units of m/s/N.
63+
public: double slip2 = 0.0;
64+
};
65+
66+
class sdf::Friction::Implementation
67+
{
68+
/// \brief The object storing contact parameters
69+
public: sdf::ODE ode;
70+
71+
/// \brief The SDF element pointer used during load.
72+
public: sdf::ElementPtr sdf{nullptr};
73+
};
74+
3675
class sdf::Surface::Implementation
3776
{
77+
/// \brief The object storing contact parameters
78+
public: sdf::Friction friction;
79+
3880
/// \brief The object storing contact parameters
3981
public: sdf::Contact contact;
4082

4183
/// \brief The SDF element pointer used during load.
4284
public: sdf::ElementPtr sdf{nullptr};
4385
};
4486

87+
88+
/////////////////////////////////////////////////
89+
ODE::ODE()
90+
: dataPtr(ignition::utils::MakeImpl<Implementation>())
91+
{
92+
}
93+
94+
/////////////////////////////////////////////////
95+
Errors ODE::Load(ElementPtr _sdf)
96+
{
97+
Errors errors;
98+
99+
this->dataPtr->sdf = _sdf;
100+
101+
// Check that sdf is a valid pointer
102+
if (!_sdf)
103+
{
104+
errors.push_back({ErrorCode::ELEMENT_MISSING,
105+
"Attempting to load a ODE, but the provided SDF "
106+
"element is null."});
107+
return errors;
108+
}
109+
110+
// Check that the provided SDF element is a <ode>
111+
// This is an error that cannot be recovered, so return an error.
112+
if (_sdf->GetName() != "ode")
113+
{
114+
errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
115+
"Attempting to load a ODE, but the provided SDF element is not a "
116+
"<ode>."});
117+
return errors;
118+
}
119+
120+
this->dataPtr->mu = _sdf->Get<double>("mu", this->dataPtr->mu).first;
121+
this->dataPtr->mu2 = _sdf->Get<double>("mu2", this->dataPtr->mu2).first;
122+
this->dataPtr->slip1 = _sdf->Get<double>("slip1", this->dataPtr->slip1).first;
123+
this->dataPtr->slip2 = _sdf->Get<double>("slip2", this->dataPtr->slip2).first;
124+
this->dataPtr->fdir1 = _sdf->Get<ignition::math::Vector3d>("fdir1",
125+
this->dataPtr->fdir1).first;
126+
127+
return errors;
128+
}
129+
130+
/////////////////////////////////////////////////
131+
double ODE::Mu() const
132+
{
133+
return this->dataPtr->mu;
134+
}
135+
136+
/////////////////////////////////////////////////
137+
void ODE::SetMu(double _mu)
138+
{
139+
this->dataPtr->mu = _mu;
140+
}
141+
142+
/////////////////////////////////////////////////
143+
double ODE::Mu2() const
144+
{
145+
return this->dataPtr->mu2;
146+
}
147+
148+
/////////////////////////////////////////////////
149+
void ODE::SetMu2(double _mu2)
150+
{
151+
this->dataPtr->mu2 = _mu2;
152+
}
153+
154+
/////////////////////////////////////////////////
155+
const ignition::math::Vector3d &ODE::Fdir1() const
156+
{
157+
return this->dataPtr->fdir1;
158+
}
159+
160+
/////////////////////////////////////////////////
161+
void ODE::SetFdir1(const ignition::math::Vector3d &_fdir)
162+
{
163+
this->dataPtr->fdir1 = _fdir;
164+
}
165+
166+
/////////////////////////////////////////////////
167+
double ODE::Slip1() const
168+
{
169+
return this->dataPtr->slip1;
170+
}
171+
172+
/////////////////////////////////////////////////
173+
void ODE::SetSlip1(double _slip1)
174+
{
175+
this->dataPtr->slip1 = _slip1;
176+
}
177+
178+
/////////////////////////////////////////////////
179+
double ODE::Slip2() const
180+
{
181+
return this->dataPtr->slip2;
182+
}
183+
184+
/////////////////////////////////////////////////
185+
void ODE::SetSlip2(double _slip2)
186+
{
187+
this->dataPtr->slip2 = _slip2;
188+
}
189+
190+
/////////////////////////////////////////////////
191+
sdf::ElementPtr ODE::Element() const
192+
{
193+
return this->dataPtr->sdf;
194+
}
195+
196+
/////////////////////////////////////////////////
197+
Friction::Friction()
198+
: dataPtr(ignition::utils::MakeImpl<Implementation>())
199+
{
200+
}
201+
202+
/////////////////////////////////////////////////
203+
Errors Friction::Load(ElementPtr _sdf)
204+
{
205+
Errors errors;
206+
207+
this->dataPtr->sdf = _sdf;
208+
209+
// Check that sdf is a valid pointer
210+
if (!_sdf)
211+
{
212+
errors.push_back({ErrorCode::ELEMENT_MISSING,
213+
"Attempting to load a Friction, but the provided SDF "
214+
"element is null."});
215+
return errors;
216+
}
217+
218+
// Check that the provided SDF element is a <friction>
219+
// This is an error that cannot be recovered, so return an error.
220+
if (_sdf->GetName() != "friction")
221+
{
222+
errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
223+
"Attempting to load a Friction, but the provided SDF element is not a "
224+
"<friction>."});
225+
return errors;
226+
}
227+
228+
if (_sdf->HasElement("ode"))
229+
{
230+
Errors err = this->dataPtr->ode.Load(_sdf->GetElement("ode"));
231+
errors.insert(errors.end(), err.begin(), err.end());
232+
}
233+
234+
return errors;
235+
}
236+
237+
/////////////////////////////////////////////////
238+
sdf::ElementPtr Friction::Element() const
239+
{
240+
return this->dataPtr->sdf;
241+
}
242+
243+
/////////////////////////////////////////////////
244+
void Friction::SetODE(const sdf::ODE &_ode)
245+
{
246+
this->dataPtr->ode = _ode;
247+
}
248+
249+
/////////////////////////////////////////////////
250+
const sdf::ODE *Friction::ODE() const
251+
{
252+
return &this->dataPtr->ode;
253+
}
254+
45255
/////////////////////////////////////////////////
46256
Contact::Contact()
47257
: dataPtr(ignition::utils::MakeImpl<Implementation>())
@@ -83,6 +293,7 @@ Errors Contact::Load(ElementPtr _sdf)
83293
// \todo(nkoenig) Parse the remaining collide properties.
84294
return errors;
85295
}
296+
86297
/////////////////////////////////////////////////
87298
sdf::ElementPtr Contact::Element() const
88299
{
@@ -139,6 +350,12 @@ Errors Surface::Load(ElementPtr _sdf)
139350
errors.insert(errors.end(), err.begin(), err.end());
140351
}
141352

353+
if (_sdf->HasElement("friction"))
354+
{
355+
Errors err = this->dataPtr->friction.Load(_sdf->GetElement("friction"));
356+
errors.insert(errors.end(), err.begin(), err.end());
357+
}
358+
142359
// \todo(nkoenig) Parse the remaining surface properties.
143360
return errors;
144361
}
@@ -154,6 +371,18 @@ const sdf::Contact *Surface::Contact() const
154371
return &this->dataPtr->contact;
155372
}
156373

374+
/////////////////////////////////////////////////
375+
void Surface::SetFriction(const sdf::Friction &_friction)
376+
{
377+
this->dataPtr->friction = _friction;
378+
}
379+
380+
/////////////////////////////////////////////////
381+
const sdf::Friction *Surface::Friction() const
382+
{
383+
return &this->dataPtr->friction;
384+
}
385+
157386
/////////////////////////////////////////////////
158387
void Surface::SetContact(const sdf::Contact &_contact)
159388
{

0 commit comments

Comments
 (0)