Skip to content

Commit f300ad0

Browse files
author
Paul Mitiguy
committed
Create MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers.
1 parent f474684 commit f300ad0

16 files changed

+169
-44
lines changed

‎multibody/tree/mobilizer.cc

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,10 @@ std::pair<Eigen::Quaternion<T>, Vector3<T>> Mobilizer<T>::GetPosePair(
1818
}
1919

2020
template <typename T>
21-
void Mobilizer<T>::MapVelocityDotToQDDot(const systems::Context<T>&,
22-
const Eigen::Ref<const VectorX<T>>&,
23-
EigenPtr<VectorX<T>>) const {
24-
// TODO(Mitiguy) remove this function when Mobilizer::MapVelocityDotToQDDot()
21+
void Mobilizer<T>::MapAccelerationToQDDot(const systems::Context<T>&,
22+
const Eigen::Ref<const VectorX<T>>&,
23+
EigenPtr<VectorX<T>>) const {
24+
// TODO(Mitiguy) remove this function when Mobilizer::MapAccelerationToQDDot()
2525
// is changed to a pure virtual function that requires override.
2626
const std::string error_message = fmt::format(
2727
"The function {}() has not been implemented for this "
@@ -31,10 +31,10 @@ void Mobilizer<T>::MapVelocityDotToQDDot(const systems::Context<T>&,
3131
}
3232

3333
template <typename T>
34-
void Mobilizer<T>::MapQDDotToVelocityDot(const systems::Context<T>&,
35-
const Eigen::Ref<const VectorX<T>>&,
36-
EigenPtr<VectorX<T>>) const {
37-
// TODO(Mitiguy) remove this function when Mobilizer::MapVelocityDotToQDDot()
34+
void Mobilizer<T>::MapQDDotToAcceleration(const systems::Context<T>&,
35+
const Eigen::Ref<const VectorX<T>>&,
36+
EigenPtr<VectorX<T>>) const {
37+
// TODO(Mitiguy) remove this function when Mobilizer::MapAccelerationToQDDot()
3838
// is changed to a pure virtual function that requires override.
3939
const std::string error_message = fmt::format(
4040
"The function {}() has not been implemented for this "

‎multibody/tree/mobilizer.h

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -621,19 +621,19 @@ class Mobilizer : public MultibodyElement<T> {
621621
// Note: Generalized positions and velocities are stored in `context`.
622622
// TODO(Mitiguy) change this function to a pure virtual function when it has
623623
// been overridden in all subclasses.
624-
virtual void MapVelocityDotToQDDot(const systems::Context<T>& context,
625-
const Eigen::Ref<const VectorX<T>>& vdot,
626-
EigenPtr<VectorX<T>> qddot) const;
624+
virtual void MapAccelerationToQDDot(const systems::Context<T>& context,
625+
const Eigen::Ref<const VectorX<T>>& vdot,
626+
EigenPtr<VectorX<T>> qddot) const;
627627

628628
// Uses the kinematic mapping `v̇ = N(q)⋅q̈` to calculate v̇ from q̈.
629629
// @param[in] qddot 2ⁿᵈ time derivatives of the generalized positions (q̈).
630630
// @param[out] vdot 1ˢᵗ time derivatives of generalized velocities (v̇).
631631
// Note: Generalized positions and velocities are stored in `context`.
632632
// TODO(Mitiguy) change this function to a pure virtual function when it has
633633
// been overridden in all subclasses.
634-
virtual void MapQDDotToVelocityDot(const systems::Context<T>& context,
635-
const Eigen::Ref<const VectorX<T>>& qddot,
636-
EigenPtr<VectorX<T>> v) const;
634+
virtual void MapQDDotToAcceleration(const systems::Context<T>& context,
635+
const Eigen::Ref<const VectorX<T>>& qddot,
636+
EigenPtr<VectorX<T>> v) const;
637637
// @}
638638

639639
// Returns a const Eigen expression of the vector of generalized positions

‎multibody/tree/planar_mobilizer.cc

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -185,6 +185,26 @@ void PlanarMobilizer<T>::MapQDotToVelocity(
185185
*v = qdot;
186186
}
187187

188+
template <typename T>
189+
void PlanarMobilizer<T>::MapAccelerationToQDDot(
190+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
191+
EigenPtr<VectorX<T>> qddot) const {
192+
DRAKE_ASSERT(vdot.size() == kNv);
193+
DRAKE_ASSERT(qddot != nullptr);
194+
DRAKE_ASSERT(qddot->size() == kNq);
195+
*qddot = vdot;
196+
}
197+
198+
template <typename T>
199+
void PlanarMobilizer<T>::MapQDDotToAcceleration(
200+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& qddot,
201+
EigenPtr<VectorX<T>> vdot) const {
202+
DRAKE_ASSERT(qddot.size() == kNq);
203+
DRAKE_ASSERT(vdot != nullptr);
204+
DRAKE_ASSERT(vdot->size() == kNv);
205+
*vdot = qddot;
206+
}
207+
188208
template <typename T>
189209
template <typename ToScalar>
190210
std::unique_ptr<Mobilizer<ToScalar>>

‎multibody/tree/planar_mobilizer.h

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -210,17 +210,25 @@ class PlanarMobilizer final : public MobilizerImpl<T, 3, 3> {
210210

211211
bool is_velocity_equal_to_qdot() const override { return true; }
212212

213-
/* Performs the identity mapping from v to qdot since, for this mobilizer,
214-
v = q̇. */
213+
// Maps v to qdot, which for this mobilizer is q̇ = v̇.
215214
void MapVelocityToQDot(const systems::Context<T>& context,
216215
const Eigen::Ref<const VectorX<T>>& v,
217-
EigenPtr<VectorX<T>> qdot) const override;
216+
EigenPtr<VectorX<T>> qdot) const final;
218217

219-
/* Performs the identity mapping from qdot to v since, for this mobilizer,
220-
v = q̇. */
218+
// Maps qdot to v, which for this mobilizer is v = q̇.
221219
void MapQDotToVelocity(const systems::Context<T>& context,
222220
const Eigen::Ref<const VectorX<T>>& qdot,
223-
EigenPtr<VectorX<T>> v) const override;
221+
EigenPtr<VectorX<T>> v) const final;
222+
223+
// Maps vdot to qddot, which for this mobilizer is q̈ = v̇.
224+
void MapAccelerationToQDDot(const systems::Context<T>& context,
225+
const Eigen::Ref<const VectorX<T>>& vdot,
226+
EigenPtr<VectorX<T>> qddot) const final;
227+
228+
// Maps qddot to vdot, which for this mobilizer is v̇ = q̈.
229+
void MapQDDotToAcceleration(const systems::Context<T>& context,
230+
const Eigen::Ref<const VectorX<T>>& qddot,
231+
EigenPtr<VectorX<T>> vdot) const final;
224232

225233
protected:
226234
void DoCalcNMatrix(const systems::Context<T>& context,

‎multibody/tree/prismatic_mobilizer.cc

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,26 @@ void PrismaticMobilizer<T>::MapQDotToVelocity(
138138
*v = qdot;
139139
}
140140

141+
template <typename T>
142+
void PrismaticMobilizer<T>::MapAccelerationToQDDot(
143+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
144+
EigenPtr<VectorX<T>> qddot) const {
145+
DRAKE_ASSERT(vdot.size() == kNv);
146+
DRAKE_ASSERT(qddot != nullptr);
147+
DRAKE_ASSERT(qddot->size() == kNq);
148+
*qddot = vdot;
149+
}
150+
151+
template <typename T>
152+
void PrismaticMobilizer<T>::MapQDDotToAcceleration(
153+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& qddot,
154+
EigenPtr<VectorX<T>> vdot) const {
155+
DRAKE_ASSERT(qddot.size() == kNq);
156+
DRAKE_ASSERT(vdot != nullptr);
157+
DRAKE_ASSERT(vdot->size() == kNv);
158+
*vdot = qddot;
159+
}
160+
141161
template <typename T>
142162
template <typename ToScalar>
143163
std::unique_ptr<Mobilizer<ToScalar>>

‎multibody/tree/prismatic_mobilizer.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,6 +192,16 @@ class PrismaticMobilizer final : public MobilizerImpl<T, 1, 1> {
192192
const Eigen::Ref<const VectorX<T>>& qdot,
193193
EigenPtr<VectorX<T>> v) const final;
194194

195+
// Maps vdot to qddot, which for this mobilizer is q̈ = v̇.
196+
void MapAccelerationToQDDot(const systems::Context<T>& context,
197+
const Eigen::Ref<const VectorX<T>>& vdot,
198+
EigenPtr<VectorX<T>> qddot) const final;
199+
200+
// Maps qddot to vdot, which for this mobilizer is v̇ = q̈.
201+
void MapQDDotToAcceleration(const systems::Context<T>& context,
202+
const Eigen::Ref<const VectorX<T>>& qddot,
203+
EigenPtr<VectorX<T>> vdot) const final;
204+
195205
protected:
196206
void DoCalcNMatrix(const systems::Context<T>& context,
197207
EigenPtr<MatrixX<T>> N) const final;

‎multibody/tree/revolute_mobilizer.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -138,7 +138,7 @@ void RevoluteMobilizer<T>::MapQDotToVelocity(
138138
}
139139

140140
template <typename T>
141-
void RevoluteMobilizer<T>::MapVelocityDotToQDDot(
141+
void RevoluteMobilizer<T>::MapAccelerationToQDDot(
142142
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
143143
EigenPtr<VectorX<T>> qddot) const {
144144
DRAKE_ASSERT(vdot.size() == kNv);
@@ -148,7 +148,7 @@ void RevoluteMobilizer<T>::MapVelocityDotToQDDot(
148148
}
149149

150150
template <typename T>
151-
void RevoluteMobilizer<T>::MapQDDotToVelocityDot(
151+
void RevoluteMobilizer<T>::MapQDDotToAcceleration(
152152
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& qddot,
153153
EigenPtr<VectorX<T>> vdot) const {
154154
DRAKE_ASSERT(qddot.size() == kNq);

‎multibody/tree/revolute_mobilizer.h

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -186,19 +186,21 @@ class RevoluteMobilizer final : public MobilizerImpl<T, 1, 1> {
186186

187187
void MapVelocityToQDot(const systems::Context<T>& context,
188188
const Eigen::Ref<const VectorX<T>>& v,
189-
EigenPtr<VectorX<T>> qdot) const override;
189+
EigenPtr<VectorX<T>> qdot) const final;
190190

191191
void MapQDotToVelocity(const systems::Context<T>& context,
192192
const Eigen::Ref<const VectorX<T>>& qdot,
193-
EigenPtr<VectorX<T>> v) const override;
193+
EigenPtr<VectorX<T>> v) const final;
194194

195-
void MapVelocityDotToQDDot(const systems::Context<T>& context,
196-
const Eigen::Ref<const VectorX<T>>& vdot,
197-
EigenPtr<VectorX<T>> qddot) const override;
195+
// Maps vdot to qddot, which for this mobilizer is q̈ = v̇.
196+
void MapAccelerationToQDDot(const systems::Context<T>& context,
197+
const Eigen::Ref<const VectorX<T>>& vdot,
198+
EigenPtr<VectorX<T>> qddot) const final;
198199

199-
void MapQDDotToVelocityDot(const systems::Context<T>& context,
200-
const Eigen::Ref<const VectorX<T>>& qddot,
201-
EigenPtr<VectorX<T>> vdot) const override;
200+
// Maps qddot to vdot, which for this mobilizer is v̇ = q̈.
201+
void MapQDDotToAcceleration(const systems::Context<T>& context,
202+
const Eigen::Ref<const VectorX<T>>& qddot,
203+
EigenPtr<VectorX<T>> vdot) const final;
202204

203205
protected:
204206
void DoCalcNMatrix(const systems::Context<T>& context,

‎multibody/tree/screw_mobilizer.cc

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -182,6 +182,26 @@ void ScrewMobilizer<T>::MapQDotToVelocity(
182182
*v = qdot;
183183
}
184184

185+
template <typename T>
186+
void ScrewMobilizer<T>::MapAccelerationToQDDot(
187+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
188+
EigenPtr<VectorX<T>> qddot) const {
189+
DRAKE_ASSERT(vdot.size() == kNv);
190+
DRAKE_ASSERT(qddot != nullptr);
191+
DRAKE_ASSERT(qddot->size() == kNq);
192+
*qddot = vdot;
193+
}
194+
195+
template <typename T>
196+
void ScrewMobilizer<T>::MapQDDotToAcceleration(
197+
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& qddot,
198+
EigenPtr<VectorX<T>> vdot) const {
199+
DRAKE_ASSERT(qddot.size() == kNq);
200+
DRAKE_ASSERT(vdot != nullptr);
201+
DRAKE_ASSERT(vdot->size() == kNv);
202+
*vdot = qddot;
203+
}
204+
185205
template <typename T>
186206
template <typename ToScalar>
187207
std::unique_ptr<Mobilizer<ToScalar>>

‎multibody/tree/screw_mobilizer.h

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -264,18 +264,26 @@ class ScrewMobilizer final : public MobilizerImpl<T, 1, 1> {
264264

265265
bool is_velocity_equal_to_qdot() const override { return true; }
266266

267-
/* Performs the identity mapping from v to qdot since, for this mobilizer,
268-
v = q̇. */
267+
// Maps v to qdot, which for this mobilizer is q̇ = v̇.
269268
void MapVelocityToQDot(const systems::Context<T>& context,
270269
const Eigen::Ref<const VectorX<T>>& v,
271270
EigenPtr<VectorX<T>> qdot) const final;
272271

273-
/* Performs the identity mapping from qdot to v since, for this mobilizer,
274-
v = q̇. */
272+
// Maps qdot to v, which for this mobilizer is v = q̇.
275273
void MapQDotToVelocity(const systems::Context<T>& context,
276274
const Eigen::Ref<const VectorX<T>>& qdot,
277275
EigenPtr<VectorX<T>> v) const final;
278276

277+
// Maps vdot to qddot, which for this mobilizer is q̈ = v̇.
278+
void MapAccelerationToQDDot(const systems::Context<T>& context,
279+
const Eigen::Ref<const VectorX<T>>& vdot,
280+
EigenPtr<VectorX<T>> qddot) const final;
281+
282+
// Maps qddot to vdot, which for this mobilizer is v̇ = q̈.
283+
void MapQDDotToAcceleration(const systems::Context<T>& context,
284+
const Eigen::Ref<const VectorX<T>>& qddot,
285+
EigenPtr<VectorX<T>> vdot) const final;
286+
279287
protected:
280288
void DoCalcNMatrix(const systems::Context<T>& context,
281289
EigenPtr<MatrixX<T>> N) const final;

‎multibody/tree/test/planar_mobilizer_test.cc

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -262,6 +262,19 @@ TEST_F(PlanarMobilizerTest, MapVelocityToQDotAndBack) {
262262
mobilizer_->MapQDotToVelocity(*context_, qdot, &v);
263263
EXPECT_TRUE(
264264
CompareMatrices(v, qdot, kTolerance, MatrixCompareType::relative));
265+
266+
// Test relationship between q̈ (2ⁿᵈ time derivatives of generalized positions)
267+
// and v̇ (1ˢᵗ time derivatives of generalized velocities) and vice-versa.
268+
Vector3d vdot(1.23, 4.56, 7.89);
269+
Vector3d qddot;
270+
mobilizer_->MapAccelerationToQDDot(*context_, vdot, &qddot);
271+
EXPECT_TRUE(
272+
CompareMatrices(qddot, vdot, kTolerance, MatrixCompareType::relative));
273+
274+
qddot = Vector3d(-std::sqrt(5), 9.87, 6.54);
275+
mobilizer_->MapQDDotToAcceleration(*context_, qddot, &vdot);
276+
EXPECT_TRUE(
277+
CompareMatrices(vdot, qddot, kTolerance, MatrixCompareType::relative));
265278
}
266279

267280
TEST_F(PlanarMobilizerTest, KinematicMapping) {

‎multibody/tree/test/prismatic_mobilizer_test.cc

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,17 @@ TEST_F(PrismaticMobilizerTest, MapVelocityToQDotAndBack) {
157157
qdot(0) = -std::sqrt(2);
158158
slider_->MapQDotToVelocity(*context_, qdot, &v);
159159
EXPECT_NEAR(v(0), qdot(0), kTolerance);
160+
161+
// Test relationship between q̈ (2ⁿᵈ time derivatives of generalized positions)
162+
// and v̇ (1ˢᵗ time derivatives of generalized velocities) and vice-versa.
163+
Vector1d vdot(1.2345);
164+
Vector1d qddot;
165+
slider_->MapAccelerationToQDDot(*context_, vdot, &qddot);
166+
EXPECT_NEAR(qddot(0), vdot(0), kTolerance);
167+
168+
qddot(0) = -std::sqrt(5);
169+
slider_->MapQDDotToAcceleration(*context_, qddot, &vdot);
170+
EXPECT_NEAR(vdot(0), qddot(0), kTolerance);
160171
}
161172

162173
TEST_F(PrismaticMobilizerTest, KinematicMapping) {

‎multibody/tree/test/revolute_mobilizer_test.cc

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -203,13 +203,15 @@ TEST_F(RevoluteMobilizerTest, MapVelocityToQDotAndBack) {
203203
mobilizer_->MapQDotToVelocity(*context_, qdot, &v);
204204
EXPECT_NEAR(v(0), qdot(0), kTolerance);
205205

206+
// Test relationship between q̈ (2ⁿᵈ time derivatives of generalized positions)
207+
// and v̇ (1ˢᵗ time derivatives of generalized velocities) and vice-versa.
206208
Vector1d vdot(1.2345);
207209
Vector1d qddot;
208-
mobilizer_->MapVelocityDotToQDDot(*context_, vdot, &qddot);
210+
mobilizer_->MapAccelerationToQDDot(*context_, vdot, &qddot);
209211
EXPECT_NEAR(qddot(0), vdot(0), kTolerance);
210212

211213
qddot(0) = -std::sqrt(5);
212-
mobilizer_->MapQDotToVelocity(*context_, qddot, &vdot);
214+
mobilizer_->MapQDDotToAcceleration(*context_, qddot, &vdot);
213215
EXPECT_NEAR(vdot(0), qddot(0), kTolerance);
214216
}
215217

‎multibody/tree/test/screw_mobilizer_test.cc

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -286,6 +286,17 @@ TEST_F(ScrewMobilizerTest, MapVelocityToQDotAndBack) {
286286
mobilizer_->MapQDotToVelocity(*context_, qdot, &v);
287287
EXPECT_TRUE(
288288
CompareMatrices(v, qdot, kTolerance, MatrixCompareType::relative));
289+
290+
// Test relationship between q̈ (2ⁿᵈ time derivatives of generalized positions)
291+
// and v̇ (1ˢᵗ time derivatives of generalized velocities) and vice-versa.
292+
Vector1d vdot(1.2345);
293+
Vector1d qddot;
294+
mobilizer_->MapAccelerationToQDDot(*context_, vdot, &qddot);
295+
EXPECT_NEAR(qddot(0), vdot(0), kTolerance);
296+
297+
qddot(0) = -std::sqrt(5);
298+
mobilizer_->MapQDDotToAcceleration(*context_, qddot, &vdot);
299+
EXPECT_NEAR(vdot(0), qddot(0), kTolerance);
289300
}
290301

291302
TEST_F(ScrewMobilizerTest, KinematicMapping) {

‎multibody/tree/weld_mobilizer.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ void WeldMobilizer<T>::MapQDotToVelocity(
7272
}
7373

7474
template <typename T>
75-
void WeldMobilizer<T>::MapVelocityDotToQDDot(
75+
void WeldMobilizer<T>::MapAccelerationToQDDot(
7676
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
7777
EigenPtr<VectorX<T>> qddot) const {
7878
DRAKE_ASSERT(vdot.size() == kNv);
@@ -81,7 +81,7 @@ void WeldMobilizer<T>::MapVelocityDotToQDDot(
8181
}
8282

8383
template <typename T>
84-
void WeldMobilizer<T>::MapQDDotToVelocityDot(
84+
void WeldMobilizer<T>::MapQDDotToAcceleration(
8585
const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& qddot,
8686
EigenPtr<VectorX<T>> vdot) const {
8787
DRAKE_ASSERT(qddot.size() == kNq);

‎multibody/tree/weld_mobilizer.h

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -104,15 +104,15 @@ class WeldMobilizer final : public MobilizerImpl<T, 0, 0> {
104104

105105
// This override is a no-op since this mobilizer has no generalized
106106
// velocities associated with it.
107-
void MapVelocityDotToQDDot(const systems::Context<T>& context,
108-
const Eigen::Ref<const VectorX<T>>& vdot,
109-
EigenPtr<VectorX<T>> qddot) const final;
107+
void MapAccelerationToQDDot(const systems::Context<T>& context,
108+
const Eigen::Ref<const VectorX<T>>& vdot,
109+
EigenPtr<VectorX<T>> qddot) const final;
110110

111111
// This override is a no-op since this mobilizer has no generalized
112112
// velocities associated with it.
113-
void MapQDDotToVelocityDot(const systems::Context<T>& context,
114-
const Eigen::Ref<const VectorX<T>>& qddot,
115-
EigenPtr<VectorX<T>> vdot) const final;
113+
void MapQDDotToAcceleration(const systems::Context<T>& context,
114+
const Eigen::Ref<const VectorX<T>>& qddot,
115+
EigenPtr<VectorX<T>> vdot) const final;
116116

117117
bool can_rotate() const final { return false; }
118118
bool can_translate() const final { return false; }

0 commit comments

Comments
 (0)