Skip to content

Commit 18d1cd6

Browse files
PeterBowmanPeterBowman
committed
Place joint id out of solution vector and ctors
1 parent 6fdf066 commit 18d1cd6

File tree

8 files changed

+141
-288
lines changed

8 files changed

+141
-288
lines changed

‎libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp

Lines changed: 22 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,8 @@ using namespace roboticslab;
1010

1111
// -----------------------------------------------------------------------------
1212

13-
PadenKahanOne::PadenKahanOne(int _id, const MatrixExponential & _exp, const KDL::Vector & _p)
14-
: id(_id),
15-
exp(_exp),
13+
PadenKahanOne::PadenKahanOne(const MatrixExponential & _exp, const KDL::Vector & _p)
14+
: exp(_exp),
1615
p(_p),
1716
axisPow(vectorPow2(exp.getAxis()))
1817
{}
@@ -21,9 +20,6 @@ PadenKahanOne::PadenKahanOne(int _id, const MatrixExponential & _exp, const KDL:
2120

2221
bool PadenKahanOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
2322
{
24-
solutions.resize(PadenKahanOne::solutions());
25-
JointIdsToSolutions jointIdsToSolutions(1);
26-
2723
KDL::Vector f = pointTransform * p;
2824
KDL::Vector k = rhs * p;
2925

@@ -37,19 +33,15 @@ bool PadenKahanOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
3733
KDL::Vector v_p = v - v_w;
3834

3935
double theta = std::atan2(KDL::dot(exp.getAxis(), u_p * v_p), KDL::dot(u_p, v_p));
40-
41-
jointIdsToSolutions[0] = {id, normalizeAngle(theta)};
42-
solutions[0] = jointIdsToSolutions;
36+
solutions = {{normalizeAngle(theta)}};
4337

4438
return KDL::Equal(u_w, v_w) && KDL::Equal(u_p.Norm(), v_p.Norm());
4539
}
4640

4741
// -----------------------------------------------------------------------------
4842

49-
PadenKahanTwo::PadenKahanTwo(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p, const KDL::Vector & _r)
50-
: id1(_id1),
51-
id2(_id2),
52-
exp1(_exp1),
43+
PadenKahanTwo::PadenKahanTwo(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p, const KDL::Vector & _r)
44+
: exp1(_exp1),
5345
exp2(_exp2),
5446
p(_p),
5547
r(_r),
@@ -63,9 +55,6 @@ PadenKahanTwo::PadenKahanTwo(int _id1, int _id2, const MatrixExponential & _exp1
6355

6456
bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
6557
{
66-
solutions.resize(PadenKahanTwo::solutions());
67-
JointIdsToSolutions jointIdsToSolution1(2), jointIdsToSolution2(2);
68-
6958
KDL::Vector f = pointTransform * p;
7059
KDL::Vector k = rhs * p;
7160

@@ -84,14 +73,10 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
8473

8574
KDL::Vector term1 = r + alpha * exp1.getAxis() + beta * exp2.getAxis();
8675

87-
double gamma2 = (std::pow(u.Norm(), 2) - std::pow(alpha, 2) - std::pow(beta, 2) - 2 * alpha * beta * axesDot) /
88-
std::pow(axesCross.Norm(), 2);
76+
double gamma2 = (std::pow(u.Norm(), 2) - std::pow(alpha, 2) - std::pow(beta, 2) - 2 * alpha * beta * axesDot) / std::pow(axesCross.Norm(), 2);
8977

9078
bool gamma2_zero = KDL::Equal(gamma2, 0.0);
9179

92-
jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id1;
93-
jointIdsToSolution1[1].first = jointIdsToSolution2[1].first = id2;
94-
9580
bool ret;
9681

9782
if (!gamma2_zero && gamma2 > 0.0)
@@ -117,11 +102,10 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
117102
double theta1_2 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
118103
double theta2_2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
119104

120-
jointIdsToSolution1[0].second = normalizeAngle(theta1_1);
121-
jointIdsToSolution1[1].second = normalizeAngle(theta2_1);
122-
123-
jointIdsToSolution2[0].second = normalizeAngle(theta1_2);
124-
jointIdsToSolution2[1].second = normalizeAngle(theta2_2);
105+
solutions = {
106+
{normalizeAngle(theta1_1), normalizeAngle(theta2_1)},
107+
{normalizeAngle(theta1_2), normalizeAngle(theta2_2)}
108+
};
125109

126110
ret = KDL::Equal(m1_p.Norm(), v_p.Norm());
127111
}
@@ -134,23 +118,24 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
134118
double theta1 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
135119
double theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
136120

137-
jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(theta1);
138-
jointIdsToSolution1[1].second = jointIdsToSolution2[1].second = normalizeAngle(theta2);
121+
double normalized1 = normalizeAngle(theta1);
122+
double normalized2 = normalizeAngle(theta2);
123+
124+
solutions = {
125+
{normalized1, normalized2},
126+
{normalized1, normalized2}
127+
};
139128

140129
ret = gamma2_zero && KDL::Equal(n1_p.Norm(), v_p.Norm());
141130
}
142131

143-
solutions[0] = jointIdsToSolution1;
144-
solutions[1] = jointIdsToSolution2;
145-
146132
return ret;
147133
}
148134

149135
// -----------------------------------------------------------------------------
150136

151-
PadenKahanThree::PadenKahanThree(int _id, const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
152-
: id(_id),
153-
exp(_exp),
137+
PadenKahanThree::PadenKahanThree(const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
138+
: exp(_exp),
154139
p(_p),
155140
k(_k),
156141
axisPow(vectorPow2(exp.getAxis()))
@@ -160,9 +145,6 @@ PadenKahanThree::PadenKahanThree(int _id, const MatrixExponential & _exp, const
160145

161146
bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
162147
{
163-
solutions.resize(PadenKahanThree::solutions());
164-
JointIdsToSolutions jointIdsToSolution1(1), jointIdsToSolution2(1);
165-
166148
KDL::Vector f = pointTransform * p;
167149
KDL::Vector rhsAsVector = rhs * p - k;
168150
double delta = rhsAsVector.Norm();
@@ -183,8 +165,6 @@ bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
183165
double betaCosAbs = std::abs(betaCos);
184166
bool beta_zero = KDL::Equal(betaCosAbs, 1.0);
185167

186-
jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id;
187-
188168
bool ret;
189169

190170
if (!beta_zero && betaCosAbs < 1.0)
@@ -195,20 +175,16 @@ bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
195175
double theta1 = alpha + beta;
196176
double theta2 = alpha - beta;
197177

198-
jointIdsToSolution1[0].second = normalizeAngle(theta1);
199-
jointIdsToSolution2[0].second = normalizeAngle(theta2);
200-
178+
solutions = {{normalizeAngle(theta1)}, {normalizeAngle(theta2)}};
201179
ret = true;
202180
}
203181
else
204182
{
205-
jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(alpha);
183+
double normalized = normalizeAngle(alpha);
184+
solutions = {{normalized}, {normalized}};
206185
ret = beta_zero;
207186
}
208187

209-
solutions[0] = jointIdsToSolution1;
210-
solutions[1] = jointIdsToSolution2;
211-
212188
return ret;
213189
}
214190

‎libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp

Lines changed: 24 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -21,37 +21,29 @@ namespace
2121

2222
// -----------------------------------------------------------------------------
2323

24-
PardosGotorOne::PardosGotorOne(int _id, const MatrixExponential & _exp, const KDL::Vector & _p)
25-
: id(_id),
26-
exp(_exp),
24+
PardosGotorOne::PardosGotorOne(const MatrixExponential & _exp, const KDL::Vector & _p)
25+
: exp(_exp),
2726
p(_p)
2827
{}
2928

3029
// -----------------------------------------------------------------------------
3130

3231
bool PardosGotorOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
3332
{
34-
solutions.resize(PardosGotorOne::solutions());
35-
JointIdsToSolutions jointIdsToSolutions(1);
36-
3733
KDL::Vector f = pointTransform * p;
3834
KDL::Vector k = rhs * p;
3935

4036
KDL::Vector diff = k - f;
4137
double theta = KDL::dot(exp.getAxis(), diff);
4238

43-
jointIdsToSolutions[0] = {id, theta};
44-
solutions[0] = jointIdsToSolutions;
45-
39+
solutions = {{theta}};
4640
return true;
4741
}
4842

4943
// -----------------------------------------------------------------------------
5044

51-
PardosGotorTwo::PardosGotorTwo(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
52-
: id1(_id1),
53-
id2(_id2),
54-
exp1(_exp1),
45+
PardosGotorTwo::PardosGotorTwo(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
46+
: exp1(_exp1),
5547
exp2(_exp2),
5648
p(_p),
5749
crossPr2(exp2.getAxis() * exp1.getAxis()),
@@ -62,9 +54,6 @@ PardosGotorTwo::PardosGotorTwo(int _id1, int _id2, const MatrixExponential & _ex
6254

6355
bool PardosGotorTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
6456
{
65-
solutions.resize(PardosGotorTwo::solutions());
66-
JointIdsToSolutions jointIdsToSolutions(2);
67-
6857
KDL::Vector f = pointTransform * p;
6958
KDL::Vector k = rhs * p;
7059

@@ -85,19 +74,15 @@ bool PardosGotorTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTrans
8574
double theta1 = KDL::dot(exp1.getAxis(), k - c);
8675
double theta2 = KDL::dot(exp2.getAxis(), c - f);
8776

88-
jointIdsToSolutions[0] = {id1, theta1};
89-
jointIdsToSolutions[1] = {id2, theta2};
90-
91-
solutions[0] = jointIdsToSolutions;
77+
solutions = {{theta1, theta2}};
9278

9379
return true;
9480
}
9581

9682
// -----------------------------------------------------------------------------
9783

98-
PardosGotorThree::PardosGotorThree(int _id, const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
99-
: id(_id),
100-
exp(_exp),
84+
PardosGotorThree::PardosGotorThree(const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
85+
: exp(_exp),
10186
p(_p),
10287
k(_k)
10388
{}
@@ -106,9 +91,6 @@ PardosGotorThree::PardosGotorThree(int _id, const MatrixExponential & _exp, cons
10691

10792
bool PardosGotorThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
10893
{
109-
solutions.resize(PardosGotorThree::solutions());
110-
JointIdsToSolutions jointIdsToSolution1(1), jointIdsToSolution2(1);
111-
11294
KDL::Vector f = pointTransform * p;
11395
KDL::Vector rhsAsVector = rhs * p - k;
11496
double delta = rhsAsVector.Norm();
@@ -119,36 +101,29 @@ bool PardosGotorThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTra
119101
double sq2 = std::pow(dotPr, 2) - std::pow(diff.Norm(), 2) + std::pow(delta, 2);
120102
bool sq2_zero = KDL::Equal(sq2, 0.0);
121103

122-
jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id;
123-
124104
bool ret;
125105

126106
if (!sq2_zero && sq2 > 0)
127107
{
128108
double sq = std::sqrt(std::abs(sq2));
129-
jointIdsToSolution1[0].second = dotPr + sq;
130-
jointIdsToSolution2[0].second = dotPr - sq;
109+
solutions = {{dotPr + sq}, {dotPr - sq}};
131110
ret = true;
132111
}
133112
else
134113
{
135114
KDL::Vector proy = vectorPow2(exp.getAxis()) * diff;
136-
jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = proy.Norm();
115+
double norm = proy.Norm();
116+
solutions = {{norm}, {norm}};
137117
ret = sq2_zero;
138118
}
139119

140-
solutions[0] = jointIdsToSolution1;
141-
solutions[1] = jointIdsToSolution2;
142-
143120
return ret;
144121
}
145122

146123
// -----------------------------------------------------------------------------
147124

148-
PardosGotorFour::PardosGotorFour(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
149-
: id1(_id1),
150-
id2(_id2),
151-
exp1(_exp1),
125+
PardosGotorFour::PardosGotorFour(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
126+
: exp1(_exp1),
152127
exp2(_exp2),
153128
p(_p),
154129
n(computeNormal(exp1, exp2)),
@@ -159,9 +134,6 @@ PardosGotorFour::PardosGotorFour(int _id1, int _id2, const MatrixExponential & _
159134

160135
bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
161136
{
162-
solutions.resize(PardosGotorFour::solutions());
163-
JointIdsToSolutions jointIdsToSolution1(2), jointIdsToSolution2(2);
164-
165137
KDL::Vector f = pointTransform * p;
166138
KDL::Vector k = rhs * p;
167139

@@ -190,9 +162,6 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
190162
double c_test = u_p_norm + v_p_norm - c_norm;
191163
bool c_zero = KDL::Equal(c_test, 0.0);
192164

193-
jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id1;
194-
jointIdsToSolution1[1].first = jointIdsToSolution2[1].first = id2;
195-
196165
bool ret;
197166

198167
if (!c_zero && c_test > 0.0)
@@ -227,11 +196,10 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
227196
double theta1_2 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
228197
double theta2_2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
229198

230-
jointIdsToSolution1[0].second = normalizeAngle(theta1_1);
231-
jointIdsToSolution1[1].second = normalizeAngle(theta2_1);
232-
233-
jointIdsToSolution2[0].second = normalizeAngle(theta1_2);
234-
jointIdsToSolution2[1].second = normalizeAngle(theta2_2);
199+
solutions = {
200+
{normalizeAngle(theta1_1), normalizeAngle(theta2_1)},
201+
{normalizeAngle(theta1_2), normalizeAngle(theta2_2)}
202+
};
235203

236204
ret = samePlane && KDL::Equal(m1_p.Norm(), v_p_norm);
237205
}
@@ -240,15 +208,17 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
240208
double theta1 = std::atan2(KDL::dot(exp1.getAxis(), c_diff * v_p), KDL::dot(c_diff, v_p));
241209
double theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * c_diff), KDL::dot(-c_diff, u_p));
242210

243-
jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(theta1);
244-
jointIdsToSolution1[1].second = jointIdsToSolution2[1].second = normalizeAngle(theta2);
211+
double normalized1 = normalizeAngle(theta1);
212+
double normalized2 = normalizeAngle(theta2);
213+
214+
solutions = {
215+
{normalized1, normalized2},
216+
{normalized1, normalized2}
217+
};
245218

246219
ret = c_zero;
247220
}
248221

249-
solutions[0] = jointIdsToSolution1;
250-
solutions[1] = jointIdsToSolution2;
251-
252222
return ret;
253223
}
254224

0 commit comments

Comments
 (0)