Skip to content

Commit d2a4f79

Browse files
jslee02jslee02
authored
Allow specifying mimic joint properties per DoF (#1684) (#1752)
1 parent 9f2d607 commit d2a4f79

15 files changed

+257
-117
lines changed

‎dart/common/Memory.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -173,11 +173,11 @@ public: \
173173
// Define static creator function that returns std::unique_ptr to the object
174174
#define DART_DEFINE_UNIQUE_OBJECT_CREATOR(class_name) \
175175
duke /*! Create unique instance of this class */ \
176-
_DART_DEFINE_OBJECT_CREATOR( \
177-
class_name, \
178-
DART_UNIQUE_PTR_CREATOR_NAME, \
179-
std::unique_ptr, \
180-
::std::make_unique)
176+
_DART_DEFINE_OBJECT_CREATOR( \
177+
class_name, \
178+
DART_UNIQUE_PTR_CREATOR_NAME, \
179+
std::unique_ptr, \
180+
::std::make_unique)
181181

182182
// Define static creator function that returns std::unique_ptr to the object
183183
// where the constructor is protected

‎dart/constraint/ConstraintSolver.cpp

+1-4
Original file line numberDiff line numberDiff line change
@@ -631,10 +631,7 @@ void ConstraintSolver::updateConstraints()
631631
&& joint->getMimicJoint())
632632
{
633633
mMimicMotorConstraints.push_back(std::make_shared<MimicMotorConstraint>(
634-
joint,
635-
joint->getMimicJoint(),
636-
joint->getMimicMultiplier(),
637-
joint->getMimicOffset()));
634+
joint, joint->getMimicDofProperties()));
638635
}
639636
}
640637
}

‎dart/constraint/JointCoulombFrictionConstraint.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,7 @@ double JointCoulombFrictionConstraint::getConstraintForceMixing()
112112
//==============================================================================
113113
void JointCoulombFrictionConstraint::update()
114114
{
115-
// Reset dimention
115+
// Reset dimension
116116
mDim = 0;
117117

118118
std::size_t dof = mJoint->getNumDofs();

‎dart/constraint/JointLimitConstraint.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,7 @@ double JointLimitConstraint::getConstraintForceMixing()
174174
//==============================================================================
175175
void JointLimitConstraint::update()
176176
{
177-
// Reset dimention
177+
// Reset dimension
178178
mDim = 0;
179179

180180
const int dof = static_cast<int>(mJoint->getNumDofs());

‎dart/constraint/MimicMotorConstraint.cpp

+10-11
Original file line numberDiff line numberDiff line change
@@ -50,21 +50,16 @@ double MimicMotorConstraint::mConstraintForceMixing = DART_CFM;
5050
//==============================================================================
5151
MimicMotorConstraint::MimicMotorConstraint(
5252
dynamics::Joint* joint,
53-
const dynamics::Joint* mimicJoint,
54-
double multiplier,
55-
double offset)
53+
const std::vector<dynamics::MimicDofProperties>& mimicDofProperties)
5654
: ConstraintBase(),
5755
mJoint(joint),
58-
mMimicJoint(mimicJoint),
59-
mMultiplier(multiplier),
60-
mOffset(offset),
56+
mMimicProps(mimicDofProperties),
6157
mBodyNode(joint->getChildBodyNode()),
6258
mAppliedImpulseIndex(0)
6359
{
6460
assert(joint);
65-
assert(mimicJoint);
61+
assert(joint->getNumDofs() <= mMimicProps.size());
6662
assert(mBodyNode);
67-
assert(joint->getNumDofs() == mimicJoint->getNumDofs());
6863

6964
mLifeTime[0] = 0;
7065
mLifeTime[1] = 0;
@@ -125,15 +120,19 @@ double MimicMotorConstraint::getConstraintForceMixing()
125120
//==============================================================================
126121
void MimicMotorConstraint::update()
127122
{
128-
// Reset dimention
123+
// Reset dimension
129124
mDim = 0;
130125

131126
std::size_t dof = mJoint->getNumDofs();
132127
for (std::size_t i = 0; i < dof; ++i)
133128
{
129+
const auto& mimicProp = mMimicProps[i];
130+
134131
double timeStep = mJoint->getSkeleton()->getTimeStep();
135-
double qError = mMimicJoint->getPosition(i) * mMultiplier + mOffset
136-
- mJoint->getPosition(i);
132+
double qError
133+
= mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex)
134+
* mimicProp.mMultiplier
135+
+ mimicProp.mOffset - mJoint->getPosition(i);
137136
double desiredVelocity = math::clip(
138137
qError / timeStep,
139138
mJoint->getVelocityLowerLimit(i),

‎dart/constraint/MimicMotorConstraint.hpp

+20-18
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,10 @@
3333
#ifndef DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_
3434
#define DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_
3535

36+
#include <vector>
37+
3638
#include "dart/constraint/ConstraintBase.hpp"
39+
#include "dart/dynamics/MimicDofProperties.hpp"
3740

3841
namespace dart {
3942

@@ -48,12 +51,14 @@ namespace constraint {
4851
class MimicMotorConstraint : public ConstraintBase
4952
{
5053
public:
51-
/// Constructor
54+
/// Constructor that creates a MimicMotorConstraint using the given
55+
/// MimicDofProperties for each dependent joint's DoF.
56+
/// \param[in] joint The dependent joint.
57+
/// \param[in] mimicDofProperties A vector of MimicDofProperties for each DoF
58+
/// of the dependent joint.
5259
explicit MimicMotorConstraint(
5360
dynamics::Joint* joint,
54-
const dynamics::Joint* mimicJoint,
55-
double multiplier = 1.0,
56-
double offset = 0.0);
61+
const std::vector<dynamics::MimicDofProperties>& mimicDofProperties);
5762

5863
/// Destructor
5964
~MimicMotorConstraint() override;
@@ -114,39 +119,36 @@ class MimicMotorConstraint : public ConstraintBase
114119
bool isActive() const override;
115120

116121
private:
117-
///
122+
/// Dependent joint whose motion is influenced by the reference joint.
118123
dynamics::Joint* mJoint;
119124

120-
///
121-
const dynamics::Joint* mMimicJoint;
122-
123-
///
124-
double mMultiplier, mOffset;
125+
/// Vector of MimicDofProperties for the dependent joint.
126+
std::vector<dynamics::MimicDofProperties> mMimicProps;
125127

126-
///
128+
/// BodyNode associated with the dependent joint.
127129
dynamics::BodyNode* mBodyNode;
128130

129-
/// Index of applied impulse
131+
/// Index of the applied impulse for the dependent joint.
130132
std::size_t mAppliedImpulseIndex;
131133

132-
///
134+
/// Array storing the lifetime of each constraint (in iterations).
133135
std::size_t mLifeTime[6];
134136
// TODO(JS): Lifetime should be considered only when we use iterative lcp
135137
// solver
136138

137-
///
139+
/// Array indicating whether each constraint is active or not.
138140
bool mActive[6];
139141

140-
///
142+
/// Array storing the negative velocity errors for each constraint.
141143
double mNegativeVelocityError[6];
142144

143-
///
145+
/// Array storing the previous values of the constraint forces.
144146
double mOldX[6];
145147

146-
///
148+
/// Array storing the upper bounds for the constraint forces.
147149
double mUpperBound[6];
148150

149-
///
151+
/// Array storing the lower bounds for the constraint forces.
150152
double mLowerBound[6];
151153

152154
/// Global constraint force mixing parameter in the range of [1e-9, 1]. The

‎dart/constraint/ServoMotorConstraint.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ double ServoMotorConstraint::getConstraintForceMixing()
113113
//==============================================================================
114114
void ServoMotorConstraint::update()
115115
{
116-
// Reset dimention
116+
// Reset dimension
117117
mDim = 0;
118118

119119
std::size_t dof = mJoint->getNumDofs();

‎dart/dynamics/Joint.cpp

+64-19
Original file line numberDiff line numberDiff line change
@@ -71,12 +71,20 @@ JointProperties::JointProperties(
7171
mT_ParentBodyToJoint(_T_ParentBodyToJoint),
7272
mT_ChildBodyToJoint(_T_ChildBodyToJoint),
7373
mIsPositionLimitEnforced(_isPositionLimitEnforced),
74-
mActuatorType(_actuatorType),
75-
mMimicJoint(_mimicJoint),
76-
mMimicMultiplier(_mimicMultiplier),
77-
mMimicOffset(_mimicOffset)
74+
mActuatorType(_actuatorType)
7875
{
79-
// Do nothing
76+
mMimicDofProps.resize(6);
77+
// TODO: Dof 6, which is the max value at the moment, is used because
78+
// JointProperties doesn't have the Dof information
79+
80+
for (auto i = 0u; i < mMimicDofProps.size(); ++i)
81+
{
82+
auto& prop = mMimicDofProps[i];
83+
prop.mReferenceJoint = _mimicJoint;
84+
prop.mReferenceDofIndex = i;
85+
prop.mMultiplier = _mimicMultiplier;
86+
prop.mOffset = _mimicOffset;
87+
}
8088
}
8189

8290
} // namespace detail
@@ -119,10 +127,7 @@ void Joint::setAspectProperties(const AspectProperties& properties)
119127
setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint);
120128
setLimitEnforcement(properties.mIsPositionLimitEnforced);
121129
setActuatorType(properties.mActuatorType);
122-
setMimicJoint(
123-
properties.mMimicJoint,
124-
properties.mMimicMultiplier,
125-
properties.mMimicOffset);
130+
setMimicJointDofs(properties.mMimicDofProps);
126131
}
127132

128133
//==============================================================================
@@ -206,29 +211,69 @@ Joint::ActuatorType Joint::getActuatorType() const
206211

207212
//==============================================================================
208213
void Joint::setMimicJoint(
209-
const Joint* _mimicJoint, double _mimicMultiplier, double _mimicOffset)
214+
const Joint* referenceJoint, double mimicMultiplier, double mimicOffset)
215+
{
216+
std::size_t numDofs = getNumDofs();
217+
mAspectProperties.mMimicDofProps.resize(numDofs);
218+
219+
for (std::size_t i = 0; i < numDofs; ++i)
220+
{
221+
MimicDofProperties prop;
222+
prop.mReferenceJoint = referenceJoint;
223+
prop.mReferenceDofIndex = i;
224+
prop.mMultiplier = mimicMultiplier;
225+
prop.mOffset = mimicOffset;
226+
setMimicJointDof(i, prop);
227+
}
228+
}
229+
230+
//==============================================================================
231+
void Joint::setMimicJointDof(
232+
std::size_t index, const MimicDofProperties& mimicProp)
233+
{
234+
mAspectProperties.mMimicDofProps[index] = mimicProp;
235+
}
236+
237+
//==============================================================================
238+
void Joint::setMimicJointDofs(const std::vector<MimicDofProperties>& mimicProps)
239+
{
240+
mAspectProperties.mMimicDofProps = mimicProps;
241+
}
242+
243+
//==============================================================================
244+
void Joint::setMimicJointDofs(
245+
const std::map<std::size_t, MimicDofProperties>& mimicPropMap)
246+
{
247+
for (const auto& pair : mimicPropMap)
248+
{
249+
const auto& index = pair.first;
250+
const auto& prop = pair.second;
251+
setMimicJointDof(index, prop);
252+
}
253+
}
254+
255+
//==============================================================================
256+
const Joint* Joint::getMimicJoint(std::size_t index) const
210257
{
211-
mAspectProperties.mMimicJoint = _mimicJoint;
212-
mAspectProperties.mMimicMultiplier = _mimicMultiplier;
213-
mAspectProperties.mMimicOffset = _mimicOffset;
258+
return mAspectProperties.mMimicDofProps[index].mReferenceJoint;
214259
}
215260

216261
//==============================================================================
217-
const Joint* Joint::getMimicJoint() const
262+
double Joint::getMimicMultiplier(std::size_t index) const
218263
{
219-
return mAspectProperties.mMimicJoint;
264+
return mAspectProperties.mMimicDofProps[index].mMultiplier;
220265
}
221266

222267
//==============================================================================
223-
double Joint::getMimicMultiplier() const
268+
double Joint::getMimicOffset(std::size_t index) const
224269
{
225-
return mAspectProperties.mMimicMultiplier;
270+
return mAspectProperties.mMimicDofProps[index].mOffset;
226271
}
227272

228273
//==============================================================================
229-
double Joint::getMimicOffset() const
274+
const std::vector<MimicDofProperties>& Joint::getMimicDofProperties() const
230275
{
231-
return mAspectProperties.mMimicOffset;
276+
return mAspectProperties.mMimicDofProps;
232277
}
233278

234279
//==============================================================================

‎dart/dynamics/Joint.hpp

+31-10
Original file line numberDiff line numberDiff line change
@@ -42,13 +42,15 @@
4242
#include "dart/common/Subject.hpp"
4343
#include "dart/common/VersionCounter.hpp"
4444
#include "dart/dynamics/Frame.hpp"
45+
#include "dart/dynamics/MimicDofProperties.hpp"
4546
#include "dart/dynamics/SmartPointer.hpp"
4647
#include "dart/dynamics/detail/JointAspect.hpp"
4748
#include "dart/math/MathTypes.hpp"
4849

4950
namespace dart {
5051
namespace dynamics {
5152

53+
class Joint;
5254
class BodyNode;
5355
class Skeleton;
5456
class DegreeOfFreedom;
@@ -137,20 +139,39 @@ class Joint : public virtual common::Subject,
137139
/// Get actuator type
138140
ActuatorType getActuatorType() const;
139141

140-
/// Set mimic joint
142+
/// Set the mimic joint with a single reference joint and the same multiplier
143+
/// and offset for all dependent joint's DoFs.
141144
void setMimicJoint(
142-
const Joint* _mimicJoint,
143-
double _mimicMultiplier = 1.0,
144-
double _mimicOffset = 0.0);
145+
const Joint* referenceJoint,
146+
double mimicMultiplier = 1.0,
147+
double mimicOffset = 0.0);
145148

146-
/// Get mimic joint
147-
const Joint* getMimicJoint() const;
149+
/// Sets the mimic joint properties for a specific DoF of the dependent joint.
150+
void setMimicJointDof(std::size_t index, const MimicDofProperties& mimicProp);
148151

149-
/// Get mimic joint multiplier
150-
double getMimicMultiplier() const;
152+
/// Sets the mimic joint properties for all DoFs of the dependent joint using
153+
/// a vector of MimicDofProperties.
154+
void setMimicJointDofs(const std::vector<MimicDofProperties>& mimicProps);
151155

152-
/// Get mimic joint offset
153-
double getMimicOffset() const;
156+
/// Sets the mimic joint properties for specific DoFs of the dependent joint
157+
/// using a map of DoF index and MimicDofProperties.
158+
void setMimicJointDofs(
159+
const std::map<std::size_t, MimicDofProperties>& mimicPropMap);
160+
161+
/// Returns the reference joint for the specified DoF of the dependent joint.
162+
const Joint* getMimicJoint(std::size_t index = 0) const;
163+
164+
/// Returns the mimic joint multiplier for the specified DoF of the dependent
165+
/// joint.
166+
double getMimicMultiplier(std::size_t index = 0) const;
167+
168+
/// Returns the mimic joint offset for the specified DoF of the dependent
169+
/// joint.
170+
double getMimicOffset(std::size_t index = 0) const;
171+
172+
/// Returns the vector of MimicDofProperties for all DoFs of the dependent
173+
/// joint.
174+
const std::vector<MimicDofProperties>& getMimicDofProperties() const;
154175

155176
/// Return true if this joint is kinematic joint.
156177
///

0 commit comments

Comments
 (0)