1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
|
/* SPDX-License-Identifier: GPL-2.0-or-later
* Copyright 2001-2002 NaN Holding BV. All rights reserved. */
/** \file
* \ingroup intern_iksolver
*/
#include "IK_QTask.h"
// IK_QTask
IK_QTask::IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment)
: m_size(size), m_primary(primary), m_active(active), m_segment(segment), m_weight(1.0)
{
}
// IK_QPositionTask
IK_QPositionTask::IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal)
: IK_QTask(3, primary, true, segment), m_goal(goal)
{
// computing clamping length
int num;
const IK_QSegment *seg;
m_clamp_length = 0.0;
num = 0;
for (seg = m_segment; seg; seg = seg->Parent()) {
m_clamp_length += seg->MaxExtension();
num++;
}
m_clamp_length /= 2 * num;
}
void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian)
{
// compute beta
const Vector3d &pos = m_segment->GlobalEnd();
Vector3d d_pos = m_goal - pos;
double length = d_pos.norm();
if (length > m_clamp_length)
d_pos = (m_clamp_length / length) * d_pos;
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
// compute derivatives
int i;
const IK_QSegment *seg;
for (seg = m_segment; seg; seg = seg->Parent()) {
Vector3d p = seg->GlobalStart() - pos;
for (i = 0; i < seg->NumberOfDoF(); i++) {
Vector3d axis = seg->Axis(i) * m_weight;
if (seg->Translational())
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
else {
Vector3d pa = p.cross(axis);
jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
}
}
}
}
double IK_QPositionTask::Distance() const
{
const Vector3d &pos = m_segment->GlobalEnd();
Vector3d d_pos = m_goal - pos;
return d_pos.norm();
}
// IK_QOrientationTask
IK_QOrientationTask::IK_QOrientationTask(bool primary,
const IK_QSegment *segment,
const Matrix3d &goal)
: IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
{
}
void IK_QOrientationTask::ComputeJacobian(IK_QJacobian &jacobian)
{
// compute betas
const Matrix3d &rot = m_segment->GlobalTransform().linear();
Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
Vector3d d_rot;
d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
d_rotm(0, 2) - d_rotm(2, 0),
d_rotm(1, 0) - d_rotm(0, 1));
m_distance = d_rot.norm();
jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
// compute derivatives
int i;
const IK_QSegment *seg;
for (seg = m_segment; seg; seg = seg->Parent())
for (i = 0; i < seg->NumberOfDoF(); i++) {
if (seg->Translational())
jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
else {
Vector3d axis = seg->Axis(i) * m_weight;
jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
}
}
}
// IK_QCenterOfMassTask
// NOTE: implementation not finished!
IK_QCenterOfMassTask::IK_QCenterOfMassTask(bool primary,
const IK_QSegment *segment,
const Vector3d &goal_center)
: IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
{
m_total_mass_inv = ComputeTotalMass(m_segment);
if (!FuzzyZero(m_total_mass_inv))
m_total_mass_inv = 1.0 / m_total_mass_inv;
}
double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
{
double mass = /*seg->Mass()*/ 1.0;
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
mass += ComputeTotalMass(seg);
return mass;
}
Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
{
Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
center += ComputeCenter(seg);
return center;
}
void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
Vector3d ¢er,
const IK_QSegment *segment)
{
int i;
Vector3d p = center - segment->GlobalStart();
for (i = 0; i < segment->NumberOfDoF(); i++) {
Vector3d axis = segment->Axis(i) * m_weight;
axis *= /*segment->Mass()**/ m_total_mass_inv;
if (segment->Translational())
jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
else {
Vector3d pa = axis.cross(p);
jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
}
}
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
JacobianSegment(jacobian, center, seg);
}
void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian &jacobian)
{
Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
// compute beta
Vector3d d_pos = m_goal_center - center;
m_distance = d_pos.norm();
#if 0
if (m_distance > m_clamp_length)
d_pos = (m_clamp_length / m_distance) * d_pos;
#endif
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
// compute derivatives
JacobianSegment(jacobian, center, m_segment);
}
double IK_QCenterOfMassTask::Distance() const
{
return m_distance;
}
|
