Skip to content

Commit 39a5099

Browse files
janbenderjanbender
committed
- update to Eigen 3.3.4
- added possibility to define motor target sequences in scene files
1 parent d11ffcc commit 39a5099

112 files changed

Lines changed: 2294 additions & 947 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎Changelog.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
1+
- added possibility to define motor target sequences in scene files
12
- added implementation of the paper Kugelstadt, Schoemer, "Position and Orientation Based Cosserat Rods", SCA 2016 and a corresponding demo
2-
- update to Eigen 3.3
3+
- update to Eigen 3.3.4
34

45
1.5.1
56
- added Timing class

‎Demos/Common/Config.h‎

Lines changed: 0 additions & 24 deletions
This file was deleted.

‎Demos/RigidBodyDemos/JointDemo.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -181,18 +181,18 @@ void timeStep ()
181181
SimulationModel::ConstraintVector &constraints = model.getConstraints();
182182
TargetAngleMotorHingeJoint &joint1 = (*(TargetAngleMotorHingeJoint*)constraints[8]);
183183
TargetVelocityMotorHingeJoint &joint2 = (*(TargetVelocityMotorHingeJoint*)constraints[9]);
184-
joint1.setTargetAngle(currentTargetAngle);
185-
joint2.setTargetAngularVelocity(3.5);
184+
joint1.setTarget(currentTargetAngle);
185+
joint2.setTarget(3.5);
186186

187187
const Real currentTargetPos = 1.5*sin(2.0*TimeManager::getCurrent()->getTime());
188188
TargetPositionMotorSliderJoint &joint3 = (*(TargetPositionMotorSliderJoint*)constraints[12]);
189-
joint3.setTargetPosition(currentTargetPos);
189+
joint3.setTarget(currentTargetPos);
190190

191191
Real currentTargetVel = 0.25;
192192
if (((int) (0.25*TimeManager::getCurrent()->getTime())) % 2 == 1)
193193
currentTargetVel = -currentTargetVel;
194194
TargetVelocityMotorSliderJoint &joint4 = (*(TargetVelocityMotorSliderJoint*)constraints[14]);
195-
joint4.setTargetVelocity(currentTargetVel);
195+
joint4.setTarget(currentTargetVel);
196196
}
197197
}
198198

‎Demos/SceneLoaderDemo/SceneLoaderDemo.cpp‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1159,28 +1159,36 @@ void readScene()
11591159
{
11601160
const SceneLoader::TargetAngleMotorHingeJointData &jd = data.m_targetAngleMotorHingeJointData[i];
11611161
model.addTargetAngleMotorHingeJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
1162-
((TargetAngleMotorHingeJoint*)constraints[constraints.size() - 1])->setTargetAngle(jd.m_target);
1162+
((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
1163+
((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
1164+
((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
11631165
}
11641166

11651167
for (unsigned int i = 0; i < data.m_targetVelocityMotorHingeJointData.size(); i++)
11661168
{
11671169
const SceneLoader::TargetVelocityMotorHingeJointData &jd = data.m_targetVelocityMotorHingeJointData[i];
11681170
model.addTargetVelocityMotorHingeJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
1169-
((TargetVelocityMotorHingeJoint*)constraints[constraints.size() - 1])->setTargetAngularVelocity(jd.m_target);
1171+
((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
1172+
((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
1173+
((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
11701174
}
11711175

11721176
for (unsigned int i = 0; i < data.m_targetPositionMotorSliderJointData.size(); i++)
11731177
{
11741178
const SceneLoader::TargetPositionMotorSliderJointData &jd = data.m_targetPositionMotorSliderJointData[i];
11751179
model.addTargetPositionMotorSliderJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
1176-
((TargetPositionMotorSliderJoint*)constraints[constraints.size() - 1])->setTargetPosition(jd.m_target);
1180+
((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
1181+
((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
1182+
((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
11771183
}
11781184

11791185
for (unsigned int i = 0; i < data.m_targetVelocityMotorSliderJointData.size(); i++)
11801186
{
11811187
const SceneLoader::TargetVelocityMotorSliderJointData &jd = data.m_targetVelocityMotorSliderJointData[i];
11821188
model.addTargetVelocityMotorSliderJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
1183-
((TargetVelocityMotorSliderJoint*)constraints[constraints.size() - 1])->setTargetVelocity(jd.m_target);
1189+
((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
1190+
((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
1191+
((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
11841192
}
11851193
}
11861194

‎Demos/Simulation/Constraints.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -470,7 +470,7 @@ bool TargetPositionMotorSliderJoint::solvePositionConstraint(SimulationModel &mo
470470
rb2.getPosition(),
471471
rb2.getInertiaTensorInverseW(),
472472
rb2.getRotation(),
473-
m_targetPosition,
473+
m_target,
474474
m_jointInfo,
475475
corr_x1,
476476
corr_q1,
@@ -596,7 +596,7 @@ bool TargetVelocityMotorSliderJoint::solveVelocityConstraint(SimulationModel &mo
596596
rb2.getVelocity(),
597597
rb2.getInertiaTensorInverseW(),
598598
rb2.getAngularVelocity(),
599-
m_targetVelocity,
599+
m_target,
600600
m_jointInfo,
601601
corr_v1,
602602
corr_omega1,
@@ -669,7 +669,7 @@ bool TargetAngleMotorHingeJoint::solvePositionConstraint(SimulationModel &model)
669669
rb2.getPosition(),
670670
rb2.getInertiaTensorInverseW(),
671671
rb2.getRotation(),
672-
m_targetAngle,
672+
m_target,
673673
m_jointInfo,
674674
corr_x1,
675675
corr_q1,
@@ -792,7 +792,7 @@ bool TargetVelocityMotorHingeJoint::solveVelocityConstraint(SimulationModel &mod
792792
rb2.getVelocity(),
793793
rb2.getInertiaTensorInverseW(),
794794
rb2.getAngularVelocity(),
795-
m_targetAngularVelocity,
795+
m_target,
796796
m_jointInfo,
797797
corr_v1,
798798
corr_omega1,

‎Demos/Simulation/Constraints.h‎

Lines changed: 34 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include "Common/Common.h"
55
#define _USE_MATH_DEFINES
66
#include "math.h"
7+
#include <vector>
78

89
namespace PBD
910
{
@@ -103,77 +104,85 @@ namespace PBD
103104
virtual bool solvePositionConstraint(SimulationModel &model);
104105
};
105106

106-
class TargetPositionMotorSliderJoint : public Constraint
107+
class MotorJoint: public Constraint
108+
{
109+
public:
110+
Real m_target;
111+
std::vector<Real> m_targetSequence;
112+
MotorJoint() : Constraint(2) { m_target = 0.0; }
113+
114+
virtual Real getTarget() const { return m_target; }
115+
virtual void setTarget(const Real val) { m_target = val; }
116+
117+
virtual std::vector<Real> &getTargetSequence() { return m_targetSequence; }
118+
virtual void setTargetSequence(const std::vector<Real> &val) { m_targetSequence = val; }
119+
120+
bool getRepeatSequence() const { return m_repeatSequence; }
121+
void setRepeatSequence(bool val) { m_repeatSequence = val; }
122+
123+
private:
124+
bool m_repeatSequence;
125+
};
126+
127+
class TargetPositionMotorSliderJoint : public MotorJoint
107128
{
108129
public:
109130
static int TYPE_ID;
110131
Eigen::Matrix<Real, 3, 14> m_jointInfo;
111-
Real m_targetPosition;
112132

113-
TargetPositionMotorSliderJoint() : Constraint(2) { m_targetPosition = 0.0; }
133+
TargetPositionMotorSliderJoint() : MotorJoint() {}
114134
virtual int &getTypeId() const { return TYPE_ID; }
115135

116-
Real getTargetPosition() const { return m_targetPosition; }
117-
void setTargetPosition(const Real val) { m_targetPosition = val; }
118-
119136
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Vector3r &pos, const Vector3r &axis);
120137
virtual bool updateConstraint(SimulationModel &model);
121138
virtual bool solvePositionConstraint(SimulationModel &model);
122139
};
123140

124-
class TargetVelocityMotorSliderJoint : public Constraint
141+
class TargetVelocityMotorSliderJoint : public MotorJoint
125142
{
126143
public:
127144
static int TYPE_ID;
128145
Eigen::Matrix<Real, 3, 14> m_jointInfo;
129-
Real m_targetVelocity;
130146

131-
TargetVelocityMotorSliderJoint() : Constraint(2) { m_targetVelocity = 0.0; }
147+
TargetVelocityMotorSliderJoint() : MotorJoint() {}
132148
virtual int &getTypeId() const { return TYPE_ID; }
133149

134-
Real getTargetVelocity() const { return m_targetVelocity; }
135-
void setTargetVelocity(const Real val) { m_targetVelocity = val; }
136-
137150
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Vector3r &pos, const Vector3r &axis);
138151
virtual bool updateConstraint(SimulationModel &model);
139152
virtual bool solvePositionConstraint(SimulationModel &model);
140153
virtual bool solveVelocityConstraint(SimulationModel &model);
141154
};
142155

143-
class TargetAngleMotorHingeJoint : public Constraint
156+
class TargetAngleMotorHingeJoint : public MotorJoint
144157
{
145158
public:
146159
static int TYPE_ID;
147160
Eigen::Matrix<Real, 3, 14> m_jointInfo;
148-
Real m_targetAngle;
149-
TargetAngleMotorHingeJoint() : Constraint(2) { m_targetAngle = 0.0; }
161+
TargetAngleMotorHingeJoint() : MotorJoint() {}
150162
virtual int &getTypeId() const { return TYPE_ID; }
151163

152-
Real getTargetAngle() const { return m_targetAngle; }
153-
void setTargetAngle(const Real val)
164+
virtual void setTarget(const Real val)
154165
{
155166
const Real pi = (Real)M_PI;
156-
m_targetAngle = std::max(val, -pi);
157-
m_targetAngle = std::min(m_targetAngle, pi);
167+
m_target = std::max(val, -pi);
168+
m_target = std::min(m_target, pi);
158169
}
159170

160171
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Vector3r &pos, const Vector3r &axis);
161172
virtual bool updateConstraint(SimulationModel &model);
162173
virtual bool solvePositionConstraint(SimulationModel &model);
174+
private:
175+
std::vector<Real> m_targetSequence;
163176
};
164177

165-
class TargetVelocityMotorHingeJoint : public Constraint
178+
class TargetVelocityMotorHingeJoint : public MotorJoint
166179
{
167180
public:
168181
static int TYPE_ID;
169182
Eigen::Matrix<Real, 3, 14> m_jointInfo;
170-
Real m_targetAngularVelocity;
171-
TargetVelocityMotorHingeJoint() : Constraint(2) { m_targetAngularVelocity = 0.0; }
183+
TargetVelocityMotorHingeJoint() : MotorJoint() {}
172184
virtual int &getTypeId() const { return TYPE_ID; }
173185

174-
Real getTargetAngularVelocity() const { return m_targetAngularVelocity; }
175-
void setTargetAngularVelocity(const Real val) { m_targetAngularVelocity = val; }
176-
177186
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Vector3r &pos, const Vector3r &axis);
178187
virtual bool updateConstraint(SimulationModel &model);
179188
virtual bool solvePositionConstraint(SimulationModel &model);

‎Demos/Simulation/TimeStepController.cpp‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,6 +127,46 @@ void TimeStepController::step(SimulationModel &model)
127127
}
128128

129129
velocityConstraintProjection(model);
130+
131+
//////////////////////////////////////////////////////////////////////////
132+
// update motor joint targets
133+
//////////////////////////////////////////////////////////////////////////
134+
SimulationModel::ConstraintVector &constraints = model.getConstraints();
135+
for (unsigned int i = 0; i < constraints.size(); i++)
136+
{
137+
if ((constraints[i]->getTypeId() == TargetAngleMotorHingeJoint::TYPE_ID) ||
138+
(constraints[i]->getTypeId() == TargetVelocityMotorHingeJoint::TYPE_ID) ||
139+
(constraints[i]->getTypeId() == TargetPositionMotorSliderJoint::TYPE_ID) ||
140+
(constraints[i]->getTypeId() == TargetVelocityMotorSliderJoint::TYPE_ID))
141+
{
142+
MotorJoint *motor = (MotorJoint*)constraints[i];
143+
const std::vector<Real> sequence = motor->getTargetSequence();
144+
if (sequence.size() > 0)
145+
{
146+
Real time = tm->getTime();
147+
const Real sequenceDuration = sequence[sequence.size() - 2] - sequence[0];
148+
if (motor->getRepeatSequence())
149+
{
150+
while (time > sequenceDuration)
151+
time -= sequenceDuration;
152+
}
153+
unsigned int index = 0;
154+
while ((2*index < sequence.size()) && (sequence[2 * index] <= time))
155+
index++;
156+
157+
// linear interpolation
158+
Real target = 0.0;
159+
if (2 * index < sequence.size())
160+
{
161+
const Real alpha = (time - sequence[2 * (index - 1)]) / (sequence[2 * index] - sequence[2 * (index - 1)]);
162+
target = (1.0 - alpha) * sequence[2 * index - 1] + alpha * sequence[2 * index + 1];
163+
}
164+
else
165+
target = sequence[sequence.size() - 1];
166+
motor->setTarget(target);
167+
}
168+
}
169+
}
130170

131171
// compute new time
132172
tm->setTime (tm->getTime () + h);

0 commit comments

Comments
 (0)