blob: 7a68a8d3cc0ed730056d2219d101058a20f9d021 (
plain)
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
|
/* SPDX-License-Identifier: GPL-2.0-or-later
* Copyright 2001-2002 NaN Holding BV. All rights reserved. */
/** \file
* \ingroup intern_iksolver
*/
#pragma once
#include "IK_Math.h"
#include "IK_QJacobian.h"
#include "IK_QSegment.h"
class IK_QTask {
public:
IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment);
virtual ~IK_QTask()
{
}
int Id() const
{
return m_size;
}
void SetId(int id)
{
m_id = id;
}
int Size() const
{
return m_size;
}
bool Primary() const
{
return m_primary;
}
bool Active() const
{
return m_active;
}
double Weight() const
{
return m_weight * m_weight;
}
void SetWeight(double weight)
{
m_weight = sqrt(weight);
}
virtual void ComputeJacobian(IK_QJacobian &jacobian) = 0;
virtual double Distance() const = 0;
virtual bool PositionTask() const
{
return false;
}
virtual void Scale(double)
{
}
protected:
int m_id;
int m_size;
bool m_primary;
bool m_active;
const IK_QSegment *m_segment;
double m_weight;
};
class IK_QPositionTask : public IK_QTask {
public:
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal);
void ComputeJacobian(IK_QJacobian &jacobian);
double Distance() const;
bool PositionTask() const
{
return true;
}
void Scale(double scale)
{
m_goal *= scale;
m_clamp_length *= scale;
}
private:
Vector3d m_goal;
double m_clamp_length;
};
class IK_QOrientationTask : public IK_QTask {
public:
IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal);
double Distance() const
{
return m_distance;
}
void ComputeJacobian(IK_QJacobian &jacobian);
private:
Matrix3d m_goal;
double m_distance;
};
class IK_QCenterOfMassTask : public IK_QTask {
public:
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d ¢er);
void ComputeJacobian(IK_QJacobian &jacobian);
double Distance() const;
void Scale(double scale)
{
m_goal_center *= scale;
m_distance *= scale;
}
private:
double ComputeTotalMass(const IK_QSegment *segment);
Vector3d ComputeCenter(const IK_QSegment *segment);
void JacobianSegment(IK_QJacobian &jacobian, Vector3d ¢er, const IK_QSegment *segment);
Vector3d m_goal_center;
double m_total_mass_inv;
double m_distance;
};
|