griefith/test
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

ClangFormat: apply to source, most of intern

Browse Source 
Apply clang format as proposed in T53211.

For details on usage and instructions for migrating branches
without conflicts, see:

https://wiki.blender.org/wiki/Tools/ClangFormat
This commit is contained in:
Campbell Barton
2019-04-17 06:17:24 +02:00
parent b3dabc200a
commit e12c08e8d1
4481 changed files with 1230080 additions and 1155401 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
intern/iksolver/CMakeLists.txt
View File

@@ -19,27 +19,27 @@
# ***** END GPL LICENSE BLOCK *****
set(INC
intern
../memutil
intern
../memutil
)
set(INC_SYS
${EIGEN3_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
set(SRC
intern/IK_QJacobian.cpp
intern/IK_QJacobianSolver.cpp
intern/IK_QSegment.cpp
intern/IK_QTask.cpp
intern/IK_Solver.cpp
intern/IK_QJacobian.cpp
intern/IK_QJacobianSolver.cpp
intern/IK_QSegment.cpp
intern/IK_QTask.cpp
intern/IK_Solver.cpp
extern/IK_solver.h
intern/IK_Math.h
intern/IK_QJacobian.h
intern/IK_QJacobianSolver.h
intern/IK_QSegment.h
intern/IK_QTask.h
extern/IK_solver.h
intern/IK_Math.h
intern/IK_QJacobian.h
intern/IK_QJacobianSolver.h
intern/IK_QSegment.h
intern/IK_QTask.h
)
set(LIB

42
intern/iksolver/extern/IK_solver.h vendored
View File

@@ -22,7 +22,6 @@
* \ingroup iksolver
*/
/**
* \page IK - Blender inverse kinematics module.
*
@@ -97,28 +96,29 @@ extern "C" {
typedef void IK_Segment;
enum IK_SegmentFlag {
IK_XDOF = 1,
IK_YDOF = 2,
IK_ZDOF = 4,
IK_TRANS_XDOF = 8,
IK_TRANS_YDOF = 16,
IK_TRANS_ZDOF = 32
IK_XDOF = 1,
IK_YDOF = 2,
IK_ZDOF = 4,
IK_TRANS_XDOF = 8,
IK_TRANS_YDOF = 16,
IK_TRANS_ZDOF = 32
};
typedef enum IK_SegmentAxis {
IK_X = 0,
IK_Y = 1,
IK_Z = 2,
IK_TRANS_X = 3,
IK_TRANS_Y = 4,
IK_TRANS_Z = 5
IK_X = 0,
IK_Y = 1,
IK_Z = 2,
IK_TRANS_X = 3,
IK_TRANS_Y = 4,
IK_TRANS_Z = 5
} IK_SegmentAxis;
extern IK_Segment *IK_CreateSegment(int flag);
extern void IK_FreeSegment(IK_Segment *seg);
extern void IK_SetParent(IK_Segment *seg, IK_Segment *parent);
extern void IK_SetTransform(IK_Segment *seg, float start[3], float rest_basis[][3], float basis[][3], float length);
extern void IK_SetTransform(
IK_Segment *seg, float start[3], float rest_basis[][3], float basis[][3], float length);
extern void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax);
extern void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness);
@@ -144,8 +144,16 @@ IK_Solver *IK_CreateSolver(IK_Segment *root);
void IK_FreeSolver(IK_Solver *solver);
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight);
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight);
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle);
void IK_SolverAddGoalOrientation(IK_Solver *solver,
IK_Segment *tip,
float goal[][3],
float weight);
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
IK_Segment *tip,
float goal[3],
float polegoal[3],
float poleangle,
int getangle);
float IK_SolverGetPoleAngle(IK_Solver *solver);
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations);
@@ -158,4 +166,4 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations);
}
#endif
#endif // __IK_SOLVER_H__
#endif // __IK_SOLVER_H__

296
intern/iksolver/intern/IK_Math.h
View File

@@ -35,219 +35,227 @@ static const double IK_EPSILON = 1e-20;
static inline bool FuzzyZero(double x)
{
return fabs(x) < IK_EPSILON;
return fabs(x) < IK_EPSILON;
}
static inline double Clamp(const double x, const double min, const double max)
{
return (x < min) ? min : (x > max) ? max : x;
return (x < min) ? min : (x > max) ? max : x;
}
static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz)
static inline Eigen::Matrix3d CreateMatrix(double xx,
double xy,
double xz,
double yx,
double yy,
double yz,
double zx,
double zy,
double zz)
{
Eigen::Matrix3d M;
M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz;
M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz;
M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz;
return M;
Eigen::Matrix3d M;
M(0, 0) = xx;
M(0, 1) = xy;
M(0, 2) = xz;
M(1, 0) = yx;
M(1, 1) = yy;
M(1, 2) = yz;
M(2, 0) = zx;
M(2, 1) = zy;
M(2, 2) = zz;
return M;
}
static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
{
if (axis == 0)
return CreateMatrix(1.0, 0.0, 0.0,
0.0, cosine, -sine,
0.0, sine, cosine);
else if (axis == 1)
return CreateMatrix(cosine, 0.0, sine,
0.0, 1.0, 0.0,
-sine, 0.0, cosine);
else
return CreateMatrix(cosine, -sine, 0.0,
sine, cosine, 0.0,
0.0, 0.0, 1.0);
if (axis == 0)
return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine);
else if (axis == 1)
return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine);
else
return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
}
static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
{
return RotationMatrix(sin(angle), cos(angle), axis);
return RotationMatrix(sin(angle), cos(angle), axis);
}
static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis)
static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis)
{
double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
if (t > 16.0 * IK_EPSILON) {
if (axis == 0) return -atan2(R(1, 2), R(2, 2));
else if (axis == 1) return atan2(-R(0, 2), t);
else return -atan2(R(0, 1), R(0, 0));
}
else {
if (axis == 0) return -atan2(-R(2, 1), R(1, 1));
else if (axis == 1) return atan2(-R(0, 2), t);
else return 0.0f;
}
if (t > 16.0 * IK_EPSILON) {
if (axis == 0)
return -atan2(R(1, 2), R(2, 2));
else if (axis == 1)
return atan2(-R(0, 2), t);
else
return -atan2(R(0, 1), R(0, 0));
}
else {
if (axis == 0)
return -atan2(-R(2, 1), R(1, 1));
else if (axis == 1)
return atan2(-R(0, 2), t);
else
return 0.0f;
}
}
static inline double safe_acos(double f)
{
// acos that does not return NaN with rounding errors
if (f <= -1.0)
return M_PI;
else if (f >= 1.0)
return 0.0;
else
return acos(f);
// acos that does not return NaN with rounding errors
if (f <= -1.0)
return M_PI;
else if (f >= 1.0)
return 0.0;
else
return acos(f);
}
static inline Eigen::Vector3d normalize(const Eigen::Vector3d& v)
static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v)
{
// a sane normalize function that doesn't give (1, 0, 0) in case
// of a zero length vector
double len = v.norm();
return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
// a sane normalize function that doesn't give (1, 0, 0) in case
// of a zero length vector
double len = v.norm();
return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
}
static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
static inline double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2)
{
return safe_acos(v1.dot(v2));
return safe_acos(v1.dot(v2));
}
static inline double ComputeTwist(const Eigen::Matrix3d& R)
static inline double ComputeTwist(const Eigen::Matrix3d &R)
{
// qy and qw are the y and w components of the quaternion from R
double qy = R(0, 2) - R(2, 0);
double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
// qy and qw are the y and w components of the quaternion from R
double qy = R(0, 2) - R(2, 0);
double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
double tau = 2.0 * atan2(qy, qw);
double tau = 2.0 * atan2(qy, qw);
return tau;
return tau;
}
static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
{
return RotationMatrix(tau, 1);
return RotationMatrix(tau, 1);
}
static inline void RemoveTwist(Eigen::Matrix3d& R)
static inline void RemoveTwist(Eigen::Matrix3d &R)
{
// compute twist parameter
double tau = ComputeTwist(R);
// compute twist parameter
double tau = ComputeTwist(R);
// compute twist matrix
Eigen::Matrix3d T = ComputeTwistMatrix(tau);
// compute twist matrix
Eigen::Matrix3d T = ComputeTwistMatrix(tau);
// remove twist
R = R * T.transpose();
// remove twist
R = R * T.transpose();
}
static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d& R)
static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
{
// compute twist parameter
double tau = ComputeTwist(R);
// compute twist parameter
double tau = ComputeTwist(R);
// compute swing parameters
double num = 2.0 * (1.0 + R(1, 1));
// compute swing parameters
double num = 2.0 * (1.0 + R(1, 1));
// singularity at pi
if (fabs(num) < IK_EPSILON)
// TODO: this does now rotation of size pi over z axis, but could
// be any axis, how to deal with this i'm not sure, maybe don't
// enforce limits at all then
return Eigen::Vector3d(0.0, tau, 1.0);
// singularity at pi
if (fabs(num) < IK_EPSILON)
// TODO: this does now rotation of size pi over z axis, but could
// be any axis, how to deal with this i'm not sure, maybe don't
// enforce limits at all then
return Eigen::Vector3d(0.0, tau, 1.0);
num = 1.0 / sqrt(num);
double ax = -R(2, 1) * num;
double az = R(0, 1) * num;
num = 1.0 / sqrt(num);
double ax = -R(2, 1) * num;
double az = R(0, 1) * num;
return Eigen::Vector3d(ax, tau, az);
return Eigen::Vector3d(ax, tau, az);
}
static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
{
// length of (ax, 0, az) = sin(theta/2)
double sine2 = ax * ax + az * az;
double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
// length of (ax, 0, az) = sin(theta/2)
double sine2 = ax * ax + az * az;
double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
// compute swing matrix
Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
// compute swing matrix
Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
return S;
return S;
}
static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R)
static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
{
Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2),
R(0, 2) - R(2, 0),
R(1, 0) - R(0, 1));
Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
double l = delta.norm();
if (!FuzzyZero(l))
delta *= c / l;
return delta;
double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
double l = delta.norm();
if (!FuzzyZero(l))
delta *= c / l;
return delta;
}
static inline bool EllipseClamp(double& ax, double& az, double *amin, double *amax)
static inline bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
{
double xlim, zlim, x, z;
double xlim, zlim, x, z;
if (ax < 0.0) {
x = -ax;
xlim = -amin[0];
}
else {
x = ax;
xlim = amax[0];
}
if (ax < 0.0) {
x = -ax;
xlim = -amin[0];
}
else {
x = ax;
xlim = amax[0];
}
if (az < 0.0) {
z = -az;
zlim = -amin[1];
}
else {
z = az;
zlim = amax[1];
}
if (az < 0.0) {
z = -az;
zlim = -amin[1];
}
else {
z = az;
zlim = amax[1];
}
if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
if (x <= xlim && z <= zlim)
return false;
if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
if (x <= xlim && z <= zlim)
return false;
if (x > xlim)
x = xlim;
if (z > zlim)
z = zlim;
}
else {
double invx = 1.0 / (xlim * xlim);
double invz = 1.0 / (zlim * zlim);
if (x > xlim)
x = xlim;
if (z > zlim)
z = zlim;
}
else {
double invx = 1.0 / (xlim * xlim);
double invz = 1.0 / (zlim * zlim);
if ((x * x * invx + z * z * invz) <= 1.0)
return false;
if ((x * x * invx + z * z * invz) <= 1.0)
return false;
if (FuzzyZero(x)) {
x = 0.0;
z = zlim;
}
else {
double rico = z / x;
double old_x = x;
x = sqrt(1.0 / (invx + invz * rico * rico));
if (old_x < 0.0)
x = -x;
z = rico * x;
}
}
if (FuzzyZero(x)) {
x = 0.0;
z = zlim;
}
else {
double rico = z / x;
double old_x = x;
x = sqrt(1.0 / (invx + invz * rico * rico));
if (old_x < 0.0)
x = -x;
z = rico * x;
}
}
ax = (ax < 0.0) ? -x : x;
az = (az < 0.0) ? -z : z;
ax = (ax < 0.0) ? -x : x;
az = (az < 0.0) ? -z : z;
return true;
return true;
}

563
intern/iksolver/intern/IK_QJacobian.cpp
View File

@@ -21,11 +21,9 @@
* \ingroup iksolver
*/
#include "IK_QJacobian.h"
IK_QJacobian::IK_QJacobian()
: m_sdls(true), m_min_damp(1.0)
IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0)
{
}
@@ -35,392 +33,393 @@ IK_QJacobian::~IK_QJacobian()
void IK_QJacobian::ArmMatrices(int dof, int task_size)
{
m_dof = dof;
m_task_size = task_size;
m_dof = dof;
m_task_size = task_size;
m_jacobian.resize(task_size, dof);
m_jacobian.setZero();
m_jacobian.resize(task_size, dof);
m_jacobian.setZero();
m_alpha.resize(dof);
m_alpha.setZero();
m_alpha.resize(dof);
m_alpha.setZero();
m_nullspace.resize(dof, dof);
m_nullspace.resize(dof, dof);
m_d_theta.resize(dof);
m_d_theta_tmp.resize(dof);
m_d_norm_weight.resize(dof);
m_d_theta.resize(dof);
m_d_theta_tmp.resize(dof);
m_d_norm_weight.resize(dof);
m_norm.resize(dof);
m_norm.setZero();
m_norm.resize(dof);
m_norm.setZero();
m_beta.resize(task_size);
m_beta.resize(task_size);
m_weight.resize(dof);
m_weight_sqrt.resize(dof);
m_weight.setOnes();
m_weight_sqrt.setOnes();
m_weight.resize(dof);
m_weight_sqrt.resize(dof);
m_weight.setOnes();
m_weight_sqrt.setOnes();
if (task_size >= dof) {
m_transpose = false;
if (task_size >= dof) {
m_transpose = false;
m_jacobian_tmp.resize(task_size, dof);
m_jacobian_tmp.resize(task_size, dof);
m_svd_u.resize(task_size, dof);
m_svd_v.resize(dof, dof);
m_svd_w.resize(dof);
m_svd_u.resize(task_size, dof);
m_svd_v.resize(dof, dof);
m_svd_w.resize(dof);
m_svd_u_beta.resize(dof);
}
else {
// use the SVD of the transpose jacobian, it works just as well
// as the original, and often allows using smaller matrices.
m_transpose = true;
m_svd_u_beta.resize(dof);
}
else {
// use the SVD of the transpose jacobian, it works just as well
// as the original, and often allows using smaller matrices.
m_transpose = true;
m_jacobian_tmp.resize(dof, task_size);
m_jacobian_tmp.resize(dof, task_size);
m_svd_u.resize(task_size, task_size);
m_svd_v.resize(dof, task_size);
m_svd_w.resize(task_size);
m_svd_u.resize(task_size, task_size);
m_svd_v.resize(dof, task_size);
m_svd_w.resize(task_size);
m_svd_u_beta.resize(task_size);
}
m_svd_u_beta.resize(task_size);
}
}
void IK_QJacobian::SetBetas(int id, int, const Vector3d& v)
void IK_QJacobian::SetBetas(int id, int, const Vector3d &v)
{
m_beta[id + 0] = v.x();
m_beta[id + 1] = v.y();
m_beta[id + 2] = v.z();
m_beta[id + 0] = v.x();
m_beta[id + 1] = v.y();
m_beta[id + 2] = v.z();
}
void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight)
void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
{
m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id];
m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id];
m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id];
m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id];
m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id];
m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id];
m_d_norm_weight[dof_id] = norm_weight;
m_d_norm_weight[dof_id] = norm_weight;
}
void IK_QJacobian::Invert()
{
if (m_transpose) {
// SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
// so J = U*W*Vt and Jinv = V*Winv*Ut
Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
m_svd_u = svd.matrixV();
m_svd_w = svd.singularValues();
m_svd_v = svd.matrixU();
}
else {
// SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
// so Jinv = V*Winv*Ut
Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
m_svd_u = svd.matrixU();
m_svd_w = svd.singularValues();
m_svd_v = svd.matrixV();
}
if (m_transpose) {
// SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
// so J = U*W*Vt and Jinv = V*Winv*Ut
Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(),
Eigen::ComputeThinU | Eigen::ComputeThinV);
m_svd_u = svd.matrixV();
m_svd_w = svd.singularValues();
m_svd_v = svd.matrixU();
}
else {
// SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
// so Jinv = V*Winv*Ut
Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
m_svd_u = svd.matrixU();
m_svd_w = svd.singularValues();
m_svd_v = svd.matrixV();
}
if (m_sdls)
InvertSDLS();
else
InvertDLS();
if (m_sdls)
InvertSDLS();
else
InvertDLS();
}
bool IK_QJacobian::ComputeNullProjection()
{
double epsilon = 1e-10;
// compute null space projection based on V
int i, j, rank = 0;
for (i = 0; i < m_svd_w.size(); i++)
if (m_svd_w[i] > epsilon)
rank++;
double epsilon = 1e-10;
if (rank < m_task_size)
return false;
// compute null space projection based on V
int i, j, rank = 0;
for (i = 0; i < m_svd_w.size(); i++)
if (m_svd_w[i] > epsilon)
rank++;
MatrixXd basis(m_svd_v.rows(), rank);
int b = 0;
if (rank < m_task_size)
return false;
for (i = 0; i < m_svd_w.size(); i++)
if (m_svd_w[i] > epsilon) {
for (j = 0; j < m_svd_v.rows(); j++)
basis(j, b) = m_svd_v(j, i);
b++;
}
m_nullspace = basis * basis.transpose();
MatrixXd basis(m_svd_v.rows(), rank);
int b = 0;
for (i = 0; i < m_nullspace.rows(); i++)
for (j = 0; j < m_nullspace.cols(); j++)
if (i == j)
m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
else
m_nullspace(i, j) = -m_nullspace(i, j);
return true;
for (i = 0; i < m_svd_w.size(); i++)
if (m_svd_w[i] > epsilon) {
for (j = 0; j < m_svd_v.rows(); j++)
basis(j, b) = m_svd_v(j, i);
b++;
}
m_nullspace = basis * basis.transpose();
for (i = 0; i < m_nullspace.rows(); i++)
for (j = 0; j < m_nullspace.cols(); j++)
if (i == j)
m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
else
m_nullspace(i, j) = -m_nullspace(i, j);
return true;
}
void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
void IK_QJacobian::SubTask(IK_QJacobian &jacobian)
{
if (!ComputeNullProjection())
return;
if (!ComputeNullProjection())
return;
// restrict lower priority jacobian
jacobian.Restrict(m_d_theta, m_nullspace);
// restrict lower priority jacobian
jacobian.Restrict(m_d_theta, m_nullspace);
// add angle update from lower priority
jacobian.Invert();
// add angle update from lower priority
jacobian.Invert();
// note: now damps secondary angles with minimum damping value from
// SDLS, to avoid shaking when the primary task is near singularities,
// doesn't work well at all
int i;
for (i = 0; i < m_d_theta.size(); i++)
m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i);
// note: now damps secondary angles with minimum damping value from
// SDLS, to avoid shaking when the primary task is near singularities,
// doesn't work well at all
int i;
for (i = 0; i < m_d_theta.size(); i++)
m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i);
}
void IK_QJacobian::Restrict(VectorXd& d_theta, MatrixXd& nullspace)
void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace)
{
// subtract part already moved by higher task from beta
m_beta = m_beta - m_jacobian * d_theta;
// subtract part already moved by higher task from beta
m_beta = m_beta - m_jacobian * d_theta;
// note: should we be using the norm of the unrestricted jacobian for SDLS?
// project jacobian on to null space of higher priority task
m_jacobian = m_jacobian * nullspace;
// note: should we be using the norm of the unrestricted jacobian for SDLS?
// project jacobian on to null space of higher priority task
m_jacobian = m_jacobian * nullspace;
}
void IK_QJacobian::InvertSDLS()
{
// Compute the dampeds least squeares pseudo inverse of J.
//
// Since J is usually not invertible (most of the times it's not even
// square), the psuedo inverse is used. This gives us a least squares
// solution.
//
// This is fine when the J*Jt is of full rank. When J*Jt is near to
// singular the least squares inverse tries to minimize |J(dtheta) - dX)|
// and doesn't try to minimize dTheta. This results in eratic changes in
// angle. The damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
//
// The selectively damped least squares (SDLS) is used here instead of the
// DLS. The SDLS damps individual singular values, instead of using a single
// damping term.
// Compute the dampeds least squeares pseudo inverse of J.
//
// Since J is usually not invertible (most of the times it's not even
// square), the psuedo inverse is used. This gives us a least squares
// solution.
//
// This is fine when the J*Jt is of full rank. When J*Jt is near to
// singular the least squares inverse tries to minimize |J(dtheta) - dX)|
// and doesn't try to minimize dTheta. This results in eratic changes in
// angle. The damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
//
// The selectively damped least squares (SDLS) is used here instead of the
// DLS. The SDLS damps individual singular values, instead of using a single
// damping term.
double max_angle_change = M_PI / 4.0;
double epsilon = 1e-10;
int i, j;
double max_angle_change = M_PI / 4.0;
double epsilon = 1e-10;
int i, j;
m_d_theta.setZero();
m_min_damp = 1.0;
m_d_theta.setZero();
m_min_damp = 1.0;
for (i = 0; i < m_dof; i++) {
m_norm[i] = 0.0;
for (j = 0; j < m_task_size; j += 3) {
double n = 0.0;
n += m_jacobian(j, i) * m_jacobian(j, i);
n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i);
n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i);
m_norm[i] += sqrt(n);
}
}
for (i = 0; i < m_dof; i++) {
m_norm[i] = 0.0;
for (j = 0; j < m_task_size; j += 3) {
double n = 0.0;
n += m_jacobian(j, i) * m_jacobian(j, i);
n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i);
n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i);
m_norm[i] += sqrt(n);
}
}
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] <= epsilon)
continue;
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] <= epsilon)
continue;
double wInv = 1.0 / m_svd_w[i];
double alpha = 0.0;
double N = 0.0;
double wInv = 1.0 / m_svd_w[i];
double alpha = 0.0;
double N = 0.0;
// compute alpha and N
for (j = 0; j < m_svd_u.rows(); j += 3) {
alpha += m_svd_u(j, i) * m_beta[j];
alpha += m_svd_u(j + 1, i) * m_beta[j + 1];
alpha += m_svd_u(j + 2, i) * m_beta[j + 2];
// compute alpha and N
for (j = 0; j < m_svd_u.rows(); j += 3) {
alpha += m_svd_u(j, i) * m_beta[j];
alpha += m_svd_u(j + 1, i) * m_beta[j + 1];
alpha += m_svd_u(j + 2, i) * m_beta[j + 2];
// note: for 1 end effector, N will always be 1, since U is
// orthogonal, .. so could be optimized
double tmp;
tmp = m_svd_u(j, i) * m_svd_u(j, i);
tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i);
tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i);
N += sqrt(tmp);
}
alpha *= wInv;
// note: for 1 end effector, N will always be 1, since U is
// orthogonal, .. so could be optimized
double tmp;
tmp = m_svd_u(j, i) * m_svd_u(j, i);
tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i);
tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i);
N += sqrt(tmp);
}
alpha *= wInv;
// compute M, dTheta and max_dtheta
double M = 0.0;
double max_dtheta = 0.0, abs_dtheta;
// compute M, dTheta and max_dtheta
double M = 0.0;
double max_dtheta = 0.0, abs_dtheta;
for (j = 0; j < m_d_theta.size(); j++) {
double v = m_svd_v(j, i);
M += fabs(v) * m_norm[j];
for (j = 0; j < m_d_theta.size(); j++) {
double v = m_svd_v(j, i);
M += fabs(v) * m_norm[j];
// compute tmporary dTheta's
m_d_theta_tmp[j] = v * alpha;
// compute tmporary dTheta's
m_d_theta_tmp[j] = v * alpha;
// find largest absolute dTheta
// multiply with weight to prevent unnecessary damping
abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
if (abs_dtheta > max_dtheta)
max_dtheta = abs_dtheta;
}
// find largest absolute dTheta
// multiply with weight to prevent unnecessary damping
abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
if (abs_dtheta > max_dtheta)
max_dtheta = abs_dtheta;
}
M *= wInv;
M *= wInv;
// compute damping term and damp the dTheta's
double gamma = max_angle_change;
if (N < M)
gamma *= N / M;
// compute damping term and damp the dTheta's
double gamma = max_angle_change;
if (N < M)
gamma *= N / M;
double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
for (j = 0; j < m_d_theta.size(); j++) {
// slight hack: we do 0.80*, so that if there is some oscillation,
// the system can still converge (for joint limits). also, it's
// better to go a little to slow than to far
double dofdamp = damp / m_weight[j];
if (dofdamp > 1.0) dofdamp = 1.0;
m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
}
for (j = 0; j < m_d_theta.size(); j++) {
// slight hack: we do 0.80*, so that if there is some oscillation,
// the system can still converge (for joint limits). also, it's
// better to go a little to slow than to far
if (damp < m_min_damp)
m_min_damp = damp;
}
double dofdamp = damp / m_weight[j];
if (dofdamp > 1.0)
dofdamp = 1.0;
// weight + prevent from doing angle updates with angles > max_angle_change
double max_angle = 0.0, abs_angle;
m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
}
for (j = 0; j < m_dof; j++) {
m_d_theta[j] *= m_weight[j];
if (damp < m_min_damp)
m_min_damp = damp;
}
abs_angle = fabs(m_d_theta[j]);
// weight + prevent from doing angle updates with angles > max_angle_change
double max_angle = 0.0, abs_angle;
if (abs_angle > max_angle)
max_angle = abs_angle;
}
if (max_angle > max_angle_change) {
double damp = (max_angle_change) / (max_angle_change + max_angle);
for (j = 0; j < m_dof; j++) {
m_d_theta[j] *= m_weight[j];
for (j = 0; j < m_dof; j++)
m_d_theta[j] *= damp;
}
abs_angle = fabs(m_d_theta[j]);
if (abs_angle > max_angle)
max_angle = abs_angle;
}
if (max_angle > max_angle_change) {
double damp = (max_angle_change) / (max_angle_change + max_angle);
for (j = 0; j < m_dof; j++)
m_d_theta[j] *= damp;
}
}
void IK_QJacobian::InvertDLS()
{
// Compute damped least squares inverse of pseudo inverse
// Compute damping term lambda
// Compute damped least squares inverse of pseudo inverse
// Compute damping term lambda
// Note when lambda is zero this is equivalent to the
// least squares solution. This is fine when the m_jjt is
// of full rank. When m_jjt is near to singular the least squares
// inverse tries to minimize |J(dtheta) - dX)| and doesn't
// try to minimize dTheta. This results in eratic changes in angle.
// Damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
// Note when lambda is zero this is equivalent to the
// least squares solution. This is fine when the m_jjt is
// of full rank. When m_jjt is near to singular the least squares
// inverse tries to minimize |J(dtheta) - dX)| and doesn't
// try to minimize dTheta. This results in eratic changes in angle.
// Damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
// We don't want to use the damped solution everywhere so we
// only increase lamda from zero as we approach a singularity.
// We don't want to use the damped solution everywhere so we
// only increase lamda from zero as we approach a singularity.
// find the smallest non-zero W value, anything below epsilon is
// treated as zero
// find the smallest non-zero W value, anything below epsilon is
// treated as zero
double epsilon = 1e-10;
double max_angle_change = 0.1;
double x_length = sqrt(m_beta.dot(m_beta));
double epsilon = 1e-10;
double max_angle_change = 0.1;
double x_length = sqrt(m_beta.dot(m_beta));
int i, j;
double w_min = std::numeric_limits<double>::max();
int i, j;
double w_min = std::numeric_limits<double>::max();
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
w_min = m_svd_w[i];
}
// compute lambda damping term
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
w_min = m_svd_w[i];
}
double d = x_length / max_angle_change;
double lambda;
// compute lambda damping term
if (w_min <= d / 2)
lambda = d / 2;
else if (w_min < d)
lambda = sqrt(w_min * (d - w_min));
else
lambda = 0.0;
double d = x_length / max_angle_change;
double lambda;
lambda *= lambda;
if (w_min <= d / 2)
lambda = d / 2;
else if (w_min < d)
lambda = sqrt(w_min * (d - w_min));
else
lambda = 0.0;
if (lambda > 10)
lambda = 10;
lambda *= lambda;
// immediately multiply with Beta, so we can do matrix*vector products
// rather than matrix*matrix products
if (lambda > 10)
lambda = 10;
// compute Ut*Beta
m_svd_u_beta = m_svd_u.transpose() * m_beta;
// immediately multiply with Beta, so we can do matrix*vector products
// rather than matrix*matrix products
m_d_theta.setZero();
// compute Ut*Beta
m_svd_u_beta = m_svd_u.transpose() * m_beta;
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon) {
double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
m_d_theta.setZero();
// compute V*Winv*Ut*Beta
m_svd_u_beta[i] *= wInv;
for (i = 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] > epsilon) {
double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
for (j = 0; j < m_d_theta.size(); j++)
m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
}
}
// compute V*Winv*Ut*Beta
m_svd_u_beta[i] *= wInv;
for (j = 0; j < m_d_theta.size(); j++)
m_d_theta[j] *= m_weight[j];
for (j = 0; j < m_d_theta.size(); j++)
m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
}
}
for (j = 0; j < m_d_theta.size(); j++)
m_d_theta[j] *= m_weight[j];
}
void IK_QJacobian::Lock(int dof_id, double delta)
{
int i;
int i;
for (i = 0; i < m_task_size; i++) {
m_beta[i] -= m_jacobian(i, dof_id) * delta;
m_jacobian(i, dof_id) = 0.0;
}
for (i = 0; i < m_task_size; i++) {
m_beta[i] -= m_jacobian(i, dof_id) * delta;
m_jacobian(i, dof_id) = 0.0;
}
m_norm[dof_id] = 0.0; // unneeded
m_d_theta[dof_id] = 0.0;
m_norm[dof_id] = 0.0; // unneeded
m_d_theta[dof_id] = 0.0;
}
double IK_QJacobian::AngleUpdate(int dof_id) const
{
return m_d_theta[dof_id];
return m_d_theta[dof_id];
}
double IK_QJacobian::AngleUpdateNorm() const
{
int i;
double mx = 0.0, dtheta_abs;
int i;
double mx = 0.0, dtheta_abs;
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
if (dtheta_abs > mx)
mx = dtheta_abs;
}
return mx;
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
if (dtheta_abs > mx)
mx = dtheta_abs;
}
return mx;
}
void IK_QJacobian::SetDoFWeight(int dof, double weight)
{
m_weight[dof] = weight;
m_weight_sqrt[dof] = sqrt(weight);
m_weight[dof] = weight;
m_weight_sqrt[dof] = sqrt(weight);
}

97
intern/iksolver/intern/IK_QJacobian.h
View File

@@ -26,72 +26,69 @@
#include "IK_Math.h"
class IK_QJacobian
{
public:
IK_QJacobian();
~IK_QJacobian();
class IK_QJacobian {
public:
IK_QJacobian();
~IK_QJacobian();
// Call once to initialize
void ArmMatrices(int dof, int task_size);
void SetDoFWeight(int dof, double weight);
// Call once to initialize
void ArmMatrices(int dof, int task_size);
void SetDoFWeight(int dof, double weight);
// Iteratively called
void SetBetas(int id, int size, const Vector3d& v);
void SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight);
// Iteratively called
void SetBetas(int id, int size, const Vector3d &v);
void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight);
void Invert();
void Invert();
double AngleUpdate(int dof_id) const;
double AngleUpdateNorm() const;
double AngleUpdate(int dof_id) const;
double AngleUpdateNorm() const;
// DoF locking for inner clamping loop
void Lock(int dof_id, double delta);
// DoF locking for inner clamping loop
void Lock(int dof_id, double delta);
// Secondary task
bool ComputeNullProjection();
// Secondary task
bool ComputeNullProjection();
void Restrict(VectorXd& d_theta, MatrixXd& nullspace);
void SubTask(IK_QJacobian& jacobian);
void Restrict(VectorXd &d_theta, MatrixXd &nullspace);
void SubTask(IK_QJacobian &jacobian);
private:
void InvertSDLS();
void InvertDLS();
private:
void InvertSDLS();
void InvertDLS();
int m_dof, m_task_size;
bool m_transpose;
int m_dof, m_task_size;
bool m_transpose;
// the jacobian matrix and it's null space projector
MatrixXd m_jacobian, m_jacobian_tmp;
MatrixXd m_nullspace;
// the jacobian matrix and it's null space projector
MatrixXd m_jacobian, m_jacobian_tmp;
MatrixXd m_nullspace;
/// the vector of intermediate betas
VectorXd m_beta;
/// the vector of intermediate betas
VectorXd m_beta;
/// the vector of computed angle changes
VectorXd m_d_theta;
VectorXd m_d_norm_weight;
/// the vector of computed angle changes
VectorXd m_d_theta;
VectorXd m_d_norm_weight;
/// space required for SVD computation
VectorXd m_svd_w;
MatrixXd m_svd_v;
MatrixXd m_svd_u;
/// space required for SVD computation
VectorXd m_svd_w;
MatrixXd m_svd_v;
MatrixXd m_svd_u;
VectorXd m_svd_u_beta;
VectorXd m_svd_u_beta;
// space required for SDLS
// space required for SDLS
bool m_sdls;
VectorXd m_norm;
VectorXd m_d_theta_tmp;
double m_min_damp;
bool m_sdls;
VectorXd m_norm;
VectorXd m_d_theta_tmp;
double m_min_damp;
// null space task vector
VectorXd m_alpha;
// null space task vector
VectorXd m_alpha;
// dof weighting
VectorXd m_weight;
VectorXd m_weight_sqrt;
// dof weighting
VectorXd m_weight;
VectorXd m_weight_sqrt;
};

519
intern/iksolver/intern/IK_QJacobianSolver.cpp
View File

@@ -21,7 +21,6 @@
* \ingroup iksolver
*/
#include <stdio.h>
#include "IK_QJacobianSolver.h"
@@ -29,340 +28,338 @@
//#include "analyze.h"
IK_QJacobianSolver::IK_QJacobianSolver()
{
m_poleconstraint = false;
m_getpoleangle = false;
m_rootmatrix.setIdentity();
m_poleconstraint = false;
m_getpoleangle = false;
m_rootmatrix.setIdentity();
}
double IK_QJacobianSolver::ComputeScale()
{
std::vector<IK_QSegment *>::iterator seg;
double length = 0.0f;
std::vector<IK_QSegment *>::iterator seg;
double length = 0.0f;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
length += (*seg)->MaxExtension();
if (length == 0.0)
return 1.0;
else
return 1.0 / length;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
length += (*seg)->MaxExtension();
if (length == 0.0)
return 1.0;
else
return 1.0 / length;
}
void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *>& tasks)
void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
{
std::list<IK_QTask *>::iterator task;
std::vector<IK_QSegment *>::iterator seg;
std::list<IK_QTask *>::iterator task;
std::vector<IK_QSegment *>::iterator seg;
for (task = tasks.begin(); task != tasks.end(); task++)
(*task)->Scale(scale);
for (task = tasks.begin(); task != tasks.end(); task++)
(*task)->Scale(scale);
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->Scale(scale);
m_rootmatrix.translation() *= scale;
m_goal *= scale;
m_polegoal *= scale;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->Scale(scale);
m_rootmatrix.translation() *= scale;
m_goal *= scale;
m_polegoal *= scale;
}
void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
{
m_segments.push_back(seg);
m_segments.push_back(seg);
IK_QSegment *child;
for (child = seg->Child(); child; child = child->Sibling())
AddSegmentList(child);
IK_QSegment *child;
for (child = seg->Child(); child; child = child->Sibling())
AddSegmentList(child);
}
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
{
m_segments.clear();
AddSegmentList(root);
m_segments.clear();
AddSegmentList(root);
// assign each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
int num_dof = 0;
// assign each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
int num_dof = 0;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->SetDoFId(num_dof);
num_dof += (*seg)->NumberOfDoF();
}
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->SetDoFId(num_dof);
num_dof += (*seg)->NumberOfDoF();
}
if (num_dof == 0)
return false;
if (num_dof == 0)
return false;
// compute task id's and assing weights to task
int primary_size = 0, primary = 0;
int secondary_size = 0, secondary = 0;
double primary_weight = 0.0, secondary_weight = 0.0;
std::list<IK_QTask *>::iterator task;
// compute task id's and assing weights to task
int primary_size = 0, primary = 0;
int secondary_size = 0, secondary = 0;
double primary_weight = 0.0, secondary_weight = 0.0;
std::list<IK_QTask *>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
if (qtask->Primary()) {
qtask->SetId(primary_size);
primary_size += qtask->Size();
primary_weight += qtask->Weight();
primary++;
}
else {
qtask->SetId(secondary_size);
secondary_size += qtask->Size();
secondary_weight += qtask->Weight();
secondary++;
}
}
if (qtask->Primary()) {
qtask->SetId(primary_size);
primary_size += qtask->Size();
primary_weight += qtask->Weight();
primary++;
}
else {
qtask->SetId(secondary_size);
secondary_size += qtask->Size();
secondary_weight += qtask->Weight();
secondary++;
}
}
if (primary_size == 0 || FuzzyZero(primary_weight))
return false;
if (primary_size == 0 || FuzzyZero(primary_weight))
return false;
m_secondary_enabled = (secondary > 0);
// rescale weights of tasks to sum up to 1
double primary_rescale = 1.0 / primary_weight;
double secondary_rescale;
if (FuzzyZero(secondary_weight))
secondary_rescale = 0.0;
else
secondary_rescale = 1.0 / secondary_weight;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
m_secondary_enabled = (secondary > 0);
if (qtask->Primary())
qtask->SetWeight(qtask->Weight() * primary_rescale);
else
qtask->SetWeight(qtask->Weight() * secondary_rescale);
}
// rescale weights of tasks to sum up to 1
double primary_rescale = 1.0 / primary_weight;
double secondary_rescale;
if (FuzzyZero(secondary_weight))
secondary_rescale = 0.0;
else
secondary_rescale = 1.0 / secondary_weight;
// set matrix sizes
m_jacobian.ArmMatrices(num_dof, primary_size);
if (secondary > 0)
m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
// set dof weights
int i;
if (qtask->Primary())
qtask->SetWeight(qtask->Weight() * primary_rescale);
else
qtask->SetWeight(qtask->Weight() * secondary_rescale);
}
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (i = 0; i < (*seg)->NumberOfDoF(); i++)
m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
// set matrix sizes
m_jacobian.ArmMatrices(num_dof, primary_size);
if (secondary > 0)
m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
return true;
// set dof weights
int i;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (i = 0; i < (*seg)->NumberOfDoF(); i++)
m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
return true;
}
void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal, Vector3d& polegoal, float poleangle, bool getangle)
void IK_QJacobianSolver::SetPoleVectorConstraint(
IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle)
{
m_poleconstraint = true;
m_poletip = tip;
m_goal = goal;
m_polegoal = polegoal;
m_poleangle = (getangle) ? 0.0f : poleangle;
m_getpoleangle = getangle;
m_poleconstraint = true;
m_poletip = tip;
m_goal = goal;
m_polegoal = polegoal;
m_poleangle = (getangle) ? 0.0f : poleangle;
m_getpoleangle = getangle;
}
void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks)
void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks)
{
// this function will be called before and after solving. calling it before
// solving gives predictable solutions by rotating towards the solution,
// and calling it afterwards ensures the solution is exact.
// this function will be called before and after solving. calling it before
// solving gives predictable solutions by rotating towards the solution,
// and calling it afterwards ensures the solution is exact.
if (!m_poleconstraint)
return;
// disable pole vector constraint in case of multiple position tasks
std::list<IK_QTask *>::iterator task;
int positiontasks = 0;
if (!m_poleconstraint)
return;
for (task = tasks.begin(); task != tasks.end(); task++)
if ((*task)->PositionTask())
positiontasks++;
if (positiontasks >= 2) {
m_poleconstraint = false;
return;
}
// disable pole vector constraint in case of multiple position tasks
std::list<IK_QTask *>::iterator task;
int positiontasks = 0;
// get positions and rotations
root->UpdateTransform(m_rootmatrix);
for (task = tasks.begin(); task != tasks.end(); task++)
if ((*task)->PositionTask())
positiontasks++;
const Vector3d rootpos = root->GlobalStart();
const Vector3d endpos = m_poletip->GlobalEnd();
const Matrix3d& rootbasis = root->GlobalTransform().linear();
if (positiontasks >= 2) {
m_poleconstraint = false;
return;
}
// construct "lookat" matrices (like gluLookAt), based on a direction and
// an up vector, with the direction going from the root to the end effector
// and the up vector going from the root to the pole constraint position.
Vector3d dir = normalize(endpos - rootpos);
Vector3d rootx = rootbasis.col(0);
Vector3d rootz = rootbasis.col(2);
Vector3d up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
// get positions and rotations
root->UpdateTransform(m_rootmatrix);
// in post, don't rotate towards the goal but only correct the pole up
Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
Vector3d poleup = normalize(m_polegoal - rootpos);
const Vector3d rootpos = root->GlobalStart();
const Vector3d endpos = m_poletip->GlobalEnd();
const Matrix3d &rootbasis = root->GlobalTransform().linear();
Matrix3d mat, polemat;
// construct "lookat" matrices (like gluLookAt), based on a direction and
// an up vector, with the direction going from the root to the end effector
// and the up vector going from the root to the pole constraint position.
Vector3d dir = normalize(endpos - rootpos);
Vector3d rootx = rootbasis.col(0);
Vector3d rootz = rootbasis.col(2);
Vector3d up = rootx * cos(m_poleangle) + rootz * sin(m_poleangle);
mat.row(0) = normalize(dir.cross(up));
mat.row(1) = mat.row(0).cross(dir);
mat.row(2) = -dir;
// in post, don't rotate towards the goal but only correct the pole up
Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
Vector3d poleup = normalize(m_polegoal - rootpos);
polemat.row(0) = normalize(poledir.cross(poleup));
polemat.row(1) = polemat.row(0).cross(poledir);
polemat.row(2) = -poledir;
Matrix3d mat, polemat;
if (m_getpoleangle) {
// we compute the pole angle that to rotate towards the target
m_poleangle = angle(mat.row(1), polemat.row(1));
mat.row(0) = normalize(dir.cross(up));
mat.row(1) = mat.row(0).cross(dir);
mat.row(2) = -dir;
double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
if (dt > 0.0)
m_poleangle = -m_poleangle;
polemat.row(0) = normalize(poledir.cross(poleup));
polemat.row(1) = polemat.row(0).cross(poledir);
polemat.row(2) = -poledir;
// solve again, with the pole angle we just computed
m_getpoleangle = false;
ConstrainPoleVector(root, tasks);
}
else {
// now we set as root matrix the difference between the current and
// desired rotation based on the pole vector constraint. we use
// transpose instead of inverse because we have orthogonal matrices
// anyway, and in case of a singular matrix we don't get NaN's.
Affine3d trans;
trans.linear() = polemat.transpose() * mat;
trans.translation() = Vector3d(0, 0, 0);
m_rootmatrix = trans * m_rootmatrix;
}
if (m_getpoleangle) {
// we compute the pole angle that to rotate towards the target
m_poleangle = angle(mat.row(1), polemat.row(1));
double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
if (dt > 0.0)
m_poleangle = -m_poleangle;
// solve again, with the pole angle we just computed
m_getpoleangle = false;
ConstrainPoleVector(root, tasks);
}
else {
// now we set as root matrix the difference between the current and
// desired rotation based on the pole vector constraint. we use
// transpose instead of inverse because we have orthogonal matrices
// anyway, and in case of a singular matrix we don't get NaN's.
Affine3d trans;
trans.linear() = polemat.transpose() * mat;
trans.translation() = Vector3d(0, 0, 0);
m_rootmatrix = trans * m_rootmatrix;
}
}
bool IK_QJacobianSolver::UpdateAngles(double& norm)
bool IK_QJacobianSolver::UpdateAngles(double &norm)
{
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
IK_QSegment *qseg, *minseg = NULL;
double minabsdelta = 1e10, absdelta;
Vector3d delta, mindelta;
bool locked = false, clamp[3];
int i, mindof = 0;
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
IK_QSegment *qseg, *minseg = NULL;
double minabsdelta = 1e10, absdelta;
Vector3d delta, mindelta;
bool locked = false, clamp[3];
int i, mindof = 0;
// here we check if any angle limits were violated. angles whose clamped
// position is the same as it was before, are locked immediate. of the
// other violation angles the most violating angle is rememberd
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
qseg = *seg;
if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
for (i = 0; i < qseg->NumberOfDoF(); i++) {
if (clamp[i] && !qseg->Locked(i)) {
absdelta = fabs(delta[i]);
// here we check if any angle limits were violated. angles whose clamped
// position is the same as it was before, are locked immediate. of the
// other violation angles the most violating angle is rememberd
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
qseg = *seg;
if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
for (i = 0; i < qseg->NumberOfDoF(); i++) {
if (clamp[i] && !qseg->Locked(i)) {
absdelta = fabs(delta[i]);
if (absdelta < IK_EPSILON) {
qseg->Lock(i, m_jacobian, delta);
locked = true;
}
else if (absdelta < minabsdelta) {
minabsdelta = absdelta;
mindelta = delta;
minseg = qseg;
mindof = i;
}
}
}
}
}
if (absdelta < IK_EPSILON) {
qseg->Lock(i, m_jacobian, delta);
locked = true;
}
else if (absdelta < minabsdelta) {
minabsdelta = absdelta;
mindelta = delta;
minseg = qseg;
mindof = i;
}
}
}
}
}
// lock most violating angle
if (minseg) {
minseg->Lock(mindof, m_jacobian, mindelta);
locked = true;
// lock most violating angle
if (minseg) {
minseg->Lock(mindof, m_jacobian, mindelta);
locked = true;
if (minabsdelta > norm)
norm = minabsdelta;
}
if (minabsdelta > norm)
norm = minabsdelta;
}
if (locked == false)
// no locking done, last inner iteration, apply the angles
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
(*seg)->UpdateAngleApply();
}
// signal if another inner iteration is needed
return locked;
if (locked == false)
// no locking done, last inner iteration, apply the angles
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
(*seg)->UpdateAngleApply();
}
// signal if another inner iteration is needed
return locked;
}
bool IK_QJacobianSolver::Solve(
IK_QSegment *root,
std::list<IK_QTask *> tasks,
const double,
const int max_iterations
)
bool IK_QJacobianSolver::Solve(IK_QSegment *root,
std::list<IK_QTask *> tasks,
const double,
const int max_iterations)
{
float scale = ComputeScale();
bool solved = false;
//double dt = analyze_time();
float scale = ComputeScale();
bool solved = false;
//double dt = analyze_time();
Scale(scale, tasks);
Scale(scale, tasks);
ConstrainPoleVector(root, tasks);
ConstrainPoleVector(root, tasks);
root->UpdateTransform(m_rootmatrix);
root->UpdateTransform(m_rootmatrix);
// iterate
for (int iterations = 0; iterations < max_iterations; iterations++) {
// update transform
root->UpdateTransform(m_rootmatrix);
// iterate
for (int iterations = 0; iterations < max_iterations; iterations++) {
// update transform
root->UpdateTransform(m_rootmatrix);
std::list<IK_QTask *>::iterator task;
std::list<IK_QTask *>::iterator task;
// compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary())
(*task)->ComputeJacobian(m_jacobian);
else
(*task)->ComputeJacobian(m_jacobian_sub);
}
// compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary())
(*task)->ComputeJacobian(m_jacobian);
else
(*task)->ComputeJacobian(m_jacobian_sub);
}
double norm = 0.0;
double norm = 0.0;
do {
// invert jacobian
try {
m_jacobian.Invert();
if (m_secondary_enabled)
m_jacobian.SubTask(m_jacobian_sub);
}
catch (...) {
fprintf(stderr, "IK Exception\n");
return false;
}
do {
// invert jacobian
try {
m_jacobian.Invert();
if (m_secondary_enabled)
m_jacobian.SubTask(m_jacobian_sub);
}
catch (...) {
fprintf(stderr, "IK Exception\n");
return false;
}
// update angles and check limits
} while (UpdateAngles(norm));
// update angles and check limits
} while (UpdateAngles(norm));
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment *>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->UnLock();
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment *>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->UnLock();
// compute angle update norm
double maxnorm = m_jacobian.AngleUpdateNorm();
if (maxnorm > norm)
norm = maxnorm;
// compute angle update norm
double maxnorm = m_jacobian.AngleUpdateNorm();
if (maxnorm > norm)
norm = maxnorm;
// check for convergence
if (norm < 1e-3 && iterations > 10) {
solved = true;
break;
}
}
// check for convergence
if (norm < 1e-3 && iterations > 10) {
solved = true;
break;
}
}
if (m_poleconstraint)
root->PrependBasis(m_rootmatrix.linear());
if (m_poleconstraint)
root->PrependBasis(m_rootmatrix.linear());
Scale(1.0f / scale, tasks);
Scale(1.0f / scale, tasks);
//analyze_add_run(max_iterations, analyze_time()-dt);
//analyze_add_run(max_iterations, analyze_time()-dt);
return solved;
return solved;
}

76
intern/iksolver/intern/IK_QJacobianSolver.h
View File

@@ -36,52 +36,52 @@
#include "IK_QSegment.h"
#include "IK_QTask.h"
class IK_QJacobianSolver
{
public:
IK_QJacobianSolver();
~IK_QJacobianSolver() {}
class IK_QJacobianSolver {
public:
IK_QJacobianSolver();
~IK_QJacobianSolver()
{
}
// setup pole vector constraint
void SetPoleVectorConstraint(IK_QSegment *tip, Vector3d& goal,
Vector3d& polegoal, float poleangle, bool getangle);
float GetPoleAngle() { return m_poleangle; }
// setup pole vector constraint
void SetPoleVectorConstraint(
IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle);
float GetPoleAngle()
{
return m_poleangle;
}
// call setup once before solving, if it fails don't solve
bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks);
// call setup once before solving, if it fails don't solve
bool Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks);
// returns true if converged, false if max number of iterations was used
bool Solve(
IK_QSegment *root,
std::list<IK_QTask*> tasks,
const double tolerance,
const int max_iterations
);
// returns true if converged, false if max number of iterations was used
bool Solve(IK_QSegment *root,
std::list<IK_QTask *> tasks,
const double tolerance,
const int max_iterations);
private:
void AddSegmentList(IK_QSegment *seg);
bool UpdateAngles(double& norm);
void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks);
private:
void AddSegmentList(IK_QSegment *seg);
bool UpdateAngles(double &norm);
void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks);
double ComputeScale();
void Scale(double scale, std::list<IK_QTask*>& tasks);
double ComputeScale();
void Scale(double scale, std::list<IK_QTask *> &tasks);
private:
private:
IK_QJacobian m_jacobian;
IK_QJacobian m_jacobian_sub;
IK_QJacobian m_jacobian;
IK_QJacobian m_jacobian_sub;
bool m_secondary_enabled;
bool m_secondary_enabled;
std::vector<IK_QSegment *> m_segments;
std::vector<IK_QSegment*> m_segments;
Affine3d m_rootmatrix;
Affine3d m_rootmatrix;
bool m_poleconstraint;
bool m_getpoleangle;
Vector3d m_goal;
Vector3d m_polegoal;
float m_poleangle;
IK_QSegment *m_poletip;
bool m_poleconstraint;
bool m_getpoleangle;
Vector3d m_goal;
Vector3d m_polegoal;
float m_poleangle;
IK_QSegment *m_poletip;
};

1061
intern/iksolver/intern/IK_QSegment.cpp
View File

@@ -21,837 +21,848 @@
* \ingroup iksolver
*/
#include "IK_QSegment.h"
// IK_QSegment
IK_QSegment::IK_QSegment(int num_DoF, bool translational)
: m_parent(NULL), m_child(NULL), m_sibling(NULL), m_composite(NULL),
m_num_DoF(num_DoF), m_translational(translational)
: m_parent(NULL),
m_child(NULL),
m_sibling(NULL),
m_composite(NULL),
m_num_DoF(num_DoF),
m_translational(translational)
{
m_locked[0] = m_locked[1] = m_locked[2] = false;
m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
m_locked[0] = m_locked[1] = m_locked[2] = false;
m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
m_max_extension = 0.0;
m_max_extension = 0.0;
m_start = Vector3d(0, 0, 0);
m_rest_basis.setIdentity();
m_basis.setIdentity();
m_translation = Vector3d(0, 0, 0);
m_start = Vector3d(0, 0, 0);
m_rest_basis.setIdentity();
m_basis.setIdentity();
m_translation = Vector3d(0, 0, 0);
m_orig_basis = m_basis;
m_orig_translation = m_translation;
m_orig_basis = m_basis;
m_orig_translation = m_translation;
}
void IK_QSegment::Reset()
{
m_locked[0] = m_locked[1] = m_locked[2] = false;
m_locked[0] = m_locked[1] = m_locked[2] = false;
m_basis = m_orig_basis;
m_translation = m_orig_translation;
SetBasis(m_basis);
m_basis = m_orig_basis;
m_translation = m_orig_translation;
SetBasis(m_basis);
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->Reset();
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->Reset();
}
void IK_QSegment::SetTransform(
const Vector3d& start,
const Matrix3d& rest_basis,
const Matrix3d& basis,
const double length
)
void IK_QSegment::SetTransform(const Vector3d &start,
const Matrix3d &rest_basis,
const Matrix3d &basis,
const double length)
{
m_max_extension = start.norm() + length;
m_max_extension = start.norm() + length;
m_start = start;
m_rest_basis = rest_basis;
m_start = start;
m_rest_basis = rest_basis;
m_orig_basis = basis;
SetBasis(basis);
m_orig_basis = basis;
SetBasis(basis);
m_translation = Vector3d(0, length, 0);
m_orig_translation = m_translation;
m_translation = Vector3d(0, length, 0);
m_orig_translation = m_translation;
}
Matrix3d IK_QSegment::BasisChange() const
{
return m_orig_basis.transpose() * m_basis;
return m_orig_basis.transpose() * m_basis;
}
Vector3d IK_QSegment::TranslationChange() const
{
return m_translation - m_orig_translation;
return m_translation - m_orig_translation;
}
IK_QSegment::~IK_QSegment()
{
if (m_parent)
m_parent->RemoveChild(this);
if (m_parent)
m_parent->RemoveChild(this);
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->m_parent = NULL;
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->m_parent = NULL;
}
void IK_QSegment::SetParent(IK_QSegment *parent)
{
if (m_parent == parent)
return;
if (m_parent)
m_parent->RemoveChild(this);
if (parent) {
m_sibling = parent->m_child;
parent->m_child = this;
}
if (m_parent == parent)
return;
m_parent = parent;
if (m_parent)
m_parent->RemoveChild(this);
if (parent) {
m_sibling = parent->m_child;
parent->m_child = this;
}
m_parent = parent;
}
void IK_QSegment::SetComposite(IK_QSegment *seg)
{
m_composite = seg;
m_composite = seg;
}
void IK_QSegment::RemoveChild(IK_QSegment *child)
{
if (m_child == NULL)
return;
else if (m_child == child)
m_child = m_child->m_sibling;
else {
IK_QSegment *seg = m_child;
if (m_child == NULL)
return;
else if (m_child == child)
m_child = m_child->m_sibling;
else {
IK_QSegment *seg = m_child;
while (seg->m_sibling != child)
seg = seg->m_sibling;
while (seg->m_sibling != child)
seg = seg->m_sibling;
if (child == seg->m_sibling)
seg->m_sibling = child->m_sibling;
}
if (child == seg->m_sibling)
seg->m_sibling = child->m_sibling;
}
}
void IK_QSegment::UpdateTransform(const Affine3d& global)
void IK_QSegment::UpdateTransform(const Affine3d &global)
{
// compute the global transform at the end of the segment
m_global_start = global.translation() + global.linear() * m_start;
// compute the global transform at the end of the segment
m_global_start = global.translation() + global.linear() * m_start;
m_global_transform.translation() = m_global_start;
m_global_transform.linear() = global.linear() * m_rest_basis * m_basis;
m_global_transform.translate(m_translation);
m_global_transform.translation() = m_global_start;
m_global_transform.linear() = global.linear() * m_rest_basis * m_basis;
m_global_transform.translate(m_translation);
// update child transforms
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->UpdateTransform(m_global_transform);
// update child transforms
for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
seg->UpdateTransform(m_global_transform);
}
void IK_QSegment::PrependBasis(const Matrix3d& mat)
void IK_QSegment::PrependBasis(const Matrix3d &mat)
{
m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
}
void IK_QSegment::Scale(double scale)
{
m_start *= scale;
m_translation *= scale;
m_orig_translation *= scale;
m_global_start *= scale;
m_global_transform.translation() *= scale;
m_max_extension *= scale;
m_start *= scale;
m_translation *= scale;
m_orig_translation *= scale;
m_global_start *= scale;
m_global_transform.translation() *= scale;
m_max_extension *= scale;
}
// IK_QSphericalSegment
IK_QSphericalSegment::IK_QSphericalSegment()
: IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
: IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
{
}
Vector3d IK_QSphericalSegment::Axis(int dof) const
{
return m_global_transform.linear().col(dof);
return m_global_transform.linear().col(dof);
}
void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
return;
if (axis == 1) {
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
if (lmin > lmax)
return;
m_min_y = lmin;
m_max_y = lmax;
if (axis == 1) {
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
m_limit_y = true;
}
else {
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
m_min_y = lmin;
m_max_y = lmax;
lmin = sin(lmin * 0.5);
lmax = sin(lmax * 0.5);
m_limit_y = true;
}
else {
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
if (axis == 0) {
m_min[0] = -lmax;
m_max[0] = -lmin;
m_limit_x = true;
}
else if (axis == 2) {
m_min[1] = -lmax;
m_max[1] = -lmin;
m_limit_z = true;
}
}
lmin = sin(lmin * 0.5);
lmax = sin(lmax * 0.5);
if (axis == 0) {
m_min[0] = -lmax;
m_max[0] = -lmin;
m_limit_x = true;
}
else if (axis == 2) {
m_min[1] = -lmax;
m_max[1] = -lmin;
m_limit_z = true;
}
}
}
void IK_QSphericalSegment::SetWeight(int axis, double weight)
{
m_weight[axis] = weight;
m_weight[axis] = weight;
}
bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1] && m_locked[2])
return false;
if (m_locked[0] && m_locked[1] && m_locked[2])
return false;
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
double theta = dq.norm();
double theta = dq.norm();
if (!FuzzyZero(theta)) {
Vector3d w = dq * (1.0 / theta);
if (!FuzzyZero(theta)) {
Vector3d w = dq * (1.0 / theta);
double sine = sin(theta);
double cosine = cos(theta);
double cosineInv = 1 - cosine;
double sine = sin(theta);
double cosine = cos(theta);
double cosineInv = 1 - cosine;
double xsine = w.x() * sine;
double ysine = w.y() * sine;
double zsine = w.z() * sine;
double xsine = w.x() * sine;
double ysine = w.y() * sine;
double zsine = w.z() * sine;
double xxcosine = w.x() * w.x() * cosineInv;
double xycosine = w.x() * w.y() * cosineInv;
double xzcosine = w.x() * w.z() * cosineInv;
double yycosine = w.y() * w.y() * cosineInv;
double yzcosine = w.y() * w.z() * cosineInv;
double zzcosine = w.z() * w.z() * cosineInv;
double xxcosine = w.x() * w.x() * cosineInv;
double xycosine = w.x() * w.y() * cosineInv;
double xzcosine = w.x() * w.z() * cosineInv;
double yycosine = w.y() * w.y() * cosineInv;
double yzcosine = w.y() * w.z() * cosineInv;
double zzcosine = w.z() * w.z() * cosineInv;
Matrix3d M = CreateMatrix(
cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
-ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
Matrix3d M = CreateMatrix(cosine + xxcosine,
-zsine + xycosine,
ysine + xzcosine,
zsine + xycosine,
cosine + yycosine,
-xsine + yzcosine,
-ysine + xzcosine,
xsine + yzcosine,
cosine + zzcosine);
m_new_basis = m_basis * M;
}
else
m_new_basis = m_basis;
m_new_basis = m_basis * M;
}
else
m_new_basis = m_basis;
if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
return false;
if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
return false;
Vector3d a = SphericalRangeParameters(m_new_basis);
Vector3d a = SphericalRangeParameters(m_new_basis);
if (m_locked[0])
a.x() = m_locked_ax;
if (m_locked[1])
a.y() = m_locked_ay;
if (m_locked[2])
a.z() = m_locked_az;
if (m_locked[0])
a.x() = m_locked_ax;
if (m_locked[1])
a.y() = m_locked_ay;
if (m_locked[2])
a.z() = m_locked_az;
double ax = a.x(), ay = a.y(), az = a.z();
double ax = a.x(), ay = a.y(), az = a.z();
clamp[0] = clamp[1] = clamp[2] = false;
if (m_limit_y) {
if (a.y() > m_max_y) {
ay = m_max_y;
clamp[1] = true;
}
else if (a.y() < m_min_y) {
ay = m_min_y;
clamp[1] = true;
}
}
clamp[0] = clamp[1] = clamp[2] = false;
if (m_limit_x && m_limit_z) {
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[2] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[2] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[2] = true;
}
}
if (m_limit_y) {
if (a.y() > m_max_y) {
ay = m_max_y;
clamp[1] = true;
}
else if (a.y() < m_min_y) {
ay = m_min_y;
clamp[1] = true;
}
}
if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
if (m_locked[0] || m_locked[1] || m_locked[2])
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
return false;
}
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
if (m_limit_x && m_limit_z) {
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[2] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[2] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[2] = true;
}
}
delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
if (m_locked[0] || m_locked[1] || m_locked[2])
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
return false;
}
if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
m_locked_ax = ax;
m_locked_az = az;
}
m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
if (!m_locked[1] && clamp[1])
m_locked_ay = ay;
return true;
delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
m_locked_ax = ax;
m_locked_az = az;
}
if (!m_locked[1] && clamp[1])
m_locked_ay = ay;
return true;
}
void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
void IK_QSphericalSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
{
if (dof == 1) {
m_locked[1] = true;
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
else {
m_locked[0] = m_locked[2] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id + 2, delta[2]);
}
if (dof == 1) {
m_locked[1] = true;
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
else {
m_locked[0] = m_locked[2] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id + 2, delta[2]);
}
}
void IK_QSphericalSegment::UpdateAngleApply()
{
m_basis = m_new_basis;
m_basis = m_new_basis;
}
// IK_QNullSegment
IK_QNullSegment::IK_QNullSegment()
: IK_QSegment(0, false)
IK_QNullSegment::IK_QNullSegment() : IK_QSegment(0, false)
{
}
// IK_QRevoluteSegment
IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
: IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
: IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
{
}
void IK_QRevoluteSegment::SetBasis(const Matrix3d& basis)
void IK_QRevoluteSegment::SetBasis(const Matrix3d &basis)
{
if (m_axis == 1) {
m_angle = ComputeTwist(basis);
m_basis = ComputeTwistMatrix(m_angle);
}
else {
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_basis = RotationMatrix(m_angle, m_axis);
}
if (m_axis == 1) {
m_angle = ComputeTwist(basis);
m_basis = ComputeTwistMatrix(m_angle);
}
else {
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_basis = RotationMatrix(m_angle, m_axis);
}
}
Vector3d IK_QRevoluteSegment::Axis(int) const
{
return m_global_transform.linear().col(m_axis);
return m_global_transform.linear().col(m_axis);
}
bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0])
return false;
if (m_locked[0])
return false;
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
clamp[0] = false;
clamp[0] = false;
if (m_limit == false)
return false;
if (m_limit == false)
return false;
if (m_new_angle > m_max)
delta[0] = m_max - m_angle;
else if (m_new_angle < m_min)
delta[0] = m_min - m_angle;
else
return false;
clamp[0] = true;
m_new_angle = m_angle + delta[0];
if (m_new_angle > m_max)
delta[0] = m_max - m_angle;
else if (m_new_angle < m_min)
delta[0] = m_min - m_angle;
else
return false;
return true;
clamp[0] = true;
m_new_angle = m_angle + delta[0];
return true;
}
void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta)
void IK_QRevoluteSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
{
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
}
void IK_QRevoluteSegment::UpdateAngleApply()
{
m_angle = m_new_angle;
m_basis = RotationMatrix(m_angle, m_axis);
m_angle = m_new_angle;
m_basis = RotationMatrix(m_angle, m_axis);
}
void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax || m_axis != axis)
return;
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
if (lmin > lmax || m_axis != axis)
return;
m_min = lmin;
m_max = lmax;
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
m_limit = true;
m_min = lmin;
m_max = lmax;
m_limit = true;
}
void IK_QRevoluteSegment::SetWeight(int axis, double weight)
{
if (axis == m_axis)
m_weight[0] = weight;
if (axis == m_axis)
m_weight[0] = weight;
}
// IK_QSwingSegment
IK_QSwingSegment::IK_QSwingSegment()
: IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
IK_QSwingSegment::IK_QSwingSegment() : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
{
}
void IK_QSwingSegment::SetBasis(const Matrix3d& basis)
void IK_QSwingSegment::SetBasis(const Matrix3d &basis)
{
m_basis = basis;
RemoveTwist(m_basis);
m_basis = basis;
RemoveTwist(m_basis);
}
Vector3d IK_QSwingSegment::Axis(int dof) const
{
return m_global_transform.linear().col((dof == 0) ? 0 : 2);
return m_global_transform.linear().col((dof == 0) ? 0 : 2);
}
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
if (m_locked[0] && m_locked[1])
return false;
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
Vector3d dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
double theta = dq.norm();
double theta = dq.norm();
if (!FuzzyZero(theta)) {
Vector3d w = dq * (1.0 / theta);
if (!FuzzyZero(theta)) {
Vector3d w = dq * (1.0 / theta);
double sine = sin(theta);
double cosine = cos(theta);
double cosineInv = 1 - cosine;
double sine = sin(theta);
double cosine = cos(theta);
double cosineInv = 1 - cosine;
double xsine = w.x() * sine;
double zsine = w.z() * sine;
double xsine = w.x() * sine;
double zsine = w.z() * sine;
double xxcosine = w.x() * w.x() * cosineInv;
double xzcosine = w.x() * w.z() * cosineInv;
double zzcosine = w.z() * w.z() * cosineInv;
double xxcosine = w.x() * w.x() * cosineInv;
double xzcosine = w.x() * w.z() * cosineInv;
double zzcosine = w.z() * w.z() * cosineInv;
Matrix3d M = CreateMatrix(
cosine + xxcosine, -zsine, xzcosine,
zsine, cosine, -xsine,
xzcosine, xsine, cosine + zzcosine);
Matrix3d M = CreateMatrix(cosine + xxcosine,
-zsine,
xzcosine,
zsine,
cosine,
-xsine,
xzcosine,
xsine,
cosine + zzcosine);
m_new_basis = m_basis * M;
m_new_basis = m_basis * M;
RemoveTwist(m_new_basis);
}
else
m_new_basis = m_basis;
RemoveTwist(m_new_basis);
}
else
m_new_basis = m_basis;
if (m_limit_x == false && m_limit_z == false)
return false;
if (m_limit_x == false && m_limit_z == false)
return false;
Vector3d a = SphericalRangeParameters(m_new_basis);
double ax = 0, az = 0;
Vector3d a = SphericalRangeParameters(m_new_basis);
double ax = 0, az = 0;
clamp[0] = clamp[1] = false;
if (m_limit_x && m_limit_z) {
ax = a.x();
az = a.z();
clamp[0] = clamp[1] = false;
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[1] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[1] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[1] = true;
}
}
if (m_limit_x && m_limit_z) {
ax = a.x();
az = a.z();
if (clamp[0] == false && clamp[1] == false)
return false;
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[1] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[1] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[1] = true;
}
}
m_new_basis = ComputeSwingMatrix(ax, az);
if (clamp[0] == false && clamp[1] == false)
return false;
delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
delta[1] = delta[2]; delta[2] = 0.0;
m_new_basis = ComputeSwingMatrix(ax, az);
return true;
delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
delta[1] = delta[2];
delta[2] = 0.0;
return true;
}
void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, Vector3d& delta)
void IK_QSwingSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
{
m_locked[0] = m_locked[1] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id + 1, delta[1]);
m_locked[0] = m_locked[1] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
void IK_QSwingSegment::UpdateAngleApply()
{
m_basis = m_new_basis;
m_basis = m_new_basis;
}
void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
return;
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
if (lmin > lmax)
return;
lmin = sin(lmin * 0.5);
lmax = sin(lmax * 0.5);
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
// put center of ellispe in the middle between min and max
double offset = 0.5 * (lmin + lmax);
//lmax = lmax - offset;
lmin = sin(lmin * 0.5);
lmax = sin(lmax * 0.5);
if (axis == 0) {
m_min[0] = -lmax;
m_max[0] = -lmin;
// put center of ellispe in the middle between min and max
double offset = 0.5 * (lmin + lmax);
//lmax = lmax - offset;
m_limit_x = true;
m_offset_x = offset;
m_max_x = lmax;
}
else if (axis == 2) {
m_min[1] = -lmax;
m_max[1] = -lmin;
if (axis == 0) {
m_min[0] = -lmax;
m_max[0] = -lmin;
m_limit_z = true;
m_offset_z = offset;
m_max_z = lmax;
}
m_limit_x = true;
m_offset_x = offset;
m_max_x = lmax;
}
else if (axis == 2) {
m_min[1] = -lmax;
m_max[1] = -lmin;
m_limit_z = true;
m_offset_z = offset;
m_max_z = lmax;
}
}
void IK_QSwingSegment::SetWeight(int axis, double weight)
{
if (axis == 0)
m_weight[0] = weight;
else if (axis == 2)
m_weight[1] = weight;
if (axis == 0)
m_weight[0] = weight;
else if (axis == 2)
m_weight[1] = weight;
}
// IK_QElbowSegment
IK_QElbowSegment::IK_QElbowSegment(int axis)
: IK_QSegment(2, false), m_axis(axis), m_twist(0.0), m_angle(0.0),
m_cos_twist(1.0), m_sin_twist(0.0), m_limit(false), m_limit_twist(false)
: IK_QSegment(2, false),
m_axis(axis),
m_twist(0.0),
m_angle(0.0),
m_cos_twist(1.0),
m_sin_twist(0.0),
m_limit(false),
m_limit_twist(false)
{
}
void IK_QElbowSegment::SetBasis(const Matrix3d& basis)
void IK_QElbowSegment::SetBasis(const Matrix3d &basis)
{
m_basis = basis;
m_basis = basis;
m_twist = ComputeTwist(m_basis);
RemoveTwist(m_basis);
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_twist = ComputeTwist(m_basis);
RemoveTwist(m_basis);
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
}
Vector3d IK_QElbowSegment::Axis(int dof) const
{
if (dof == 0) {
Vector3d v;
if (m_axis == 0)
v = Vector3d(m_cos_twist, 0, m_sin_twist);
else
v = Vector3d(-m_sin_twist, 0, m_cos_twist);
if (dof == 0) {
Vector3d v;
if (m_axis == 0)
v = Vector3d(m_cos_twist, 0, m_sin_twist);
else
v = Vector3d(-m_sin_twist, 0, m_cos_twist);
return m_global_transform.linear() * v;
}
else
return m_global_transform.linear().col(1);
return m_global_transform.linear() * v;
}
else
return m_global_transform.linear().col(1);
}
bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
if (m_locked[0] && m_locked[1])
return false;
clamp[0] = clamp[1] = false;
clamp[0] = clamp[1] = false;
if (!m_locked[0]) {
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
if (!m_locked[0]) {
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
if (m_limit) {
if (m_new_angle > m_max) {
delta[0] = m_max - m_angle;
m_new_angle = m_max;
clamp[0] = true;
}
else if (m_new_angle < m_min) {
delta[0] = m_min - m_angle;
m_new_angle = m_min;
clamp[0] = true;
}
}
}
if (m_limit) {
if (m_new_angle > m_max) {
delta[0] = m_max - m_angle;
m_new_angle = m_max;
clamp[0] = true;
}
else if (m_new_angle < m_min) {
delta[0] = m_min - m_angle;
m_new_angle = m_min;
clamp[0] = true;
}
}
}
if (!m_locked[1]) {
m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
if (!m_locked[1]) {
m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
if (m_limit_twist) {
if (m_new_twist > m_max_twist) {
delta[1] = m_max_twist - m_twist;
m_new_twist = m_max_twist;
clamp[1] = true;
}
else if (m_new_twist < m_min_twist) {
delta[1] = m_min_twist - m_twist;
m_new_twist = m_min_twist;
clamp[1] = true;
}
}
}
if (m_limit_twist) {
if (m_new_twist > m_max_twist) {
delta[1] = m_max_twist - m_twist;
m_new_twist = m_max_twist;
clamp[1] = true;
}
else if (m_new_twist < m_min_twist) {
delta[1] = m_min_twist - m_twist;
m_new_twist = m_min_twist;
clamp[1] = true;
}
}
}
return (clamp[0] || clamp[1]);
return (clamp[0] || clamp[1]);
}
void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
void IK_QElbowSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
{
if (dof == 0) {
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
}
else {
m_locked[1] = true;
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
if (dof == 0) {
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
}
else {
m_locked[1] = true;
jacobian.Lock(m_DoF_id + 1, delta[1]);
}
}
void IK_QElbowSegment::UpdateAngleApply()
{
m_angle = m_new_angle;
m_twist = m_new_twist;
m_angle = m_new_angle;
m_twist = m_new_twist;
m_sin_twist = sin(m_twist);
m_cos_twist = cos(m_twist);
m_sin_twist = sin(m_twist);
m_cos_twist = cos(m_twist);
Matrix3d A = RotationMatrix(m_angle, m_axis);
Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
Matrix3d A = RotationMatrix(m_angle, m_axis);
Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
m_basis = A * T;
m_basis = A * T;
}
void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmin > lmax)
return;
if (lmin > lmax)
return;
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
// clamp and convert to axis angle parameters
lmin = Clamp(lmin, -M_PI, M_PI);
lmax = Clamp(lmax, -M_PI, M_PI);
if (axis == 1) {
m_min_twist = lmin;
m_max_twist = lmax;
m_limit_twist = true;
}
else if (axis == m_axis) {
m_min = lmin;
m_max = lmax;
m_limit = true;
}
if (axis == 1) {
m_min_twist = lmin;
m_max_twist = lmax;
m_limit_twist = true;
}
else if (axis == m_axis) {
m_min = lmin;
m_max = lmax;
m_limit = true;
}
}
void IK_QElbowSegment::SetWeight(int axis, double weight)
{
if (axis == m_axis)
m_weight[0] = weight;
else if (axis == 1)
m_weight[1] = weight;
if (axis == m_axis)
m_weight[0] = weight;
else if (axis == 1)
m_weight[1] = weight;
}
// IK_QTranslateSegment
IK_QTranslateSegment::IK_QTranslateSegment(int axis1)
: IK_QSegment(1, true)
IK_QTranslateSegment::IK_QTranslateSegment(int axis1) : IK_QSegment(1, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
m_axis[0] = axis1;
m_axis[0] = axis1;
m_limit[0] = m_limit[1] = m_limit[2] = false;
m_limit[0] = m_limit[1] = m_limit[2] = false;
}
IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2)
: IK_QSegment(2, true)
IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) : IK_QSegment(2, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
m_axis_enabled[axis2] = true;
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
m_axis_enabled[axis1] = true;
m_axis_enabled[axis2] = true;
m_axis[0] = axis1;
m_axis[1] = axis2;
m_axis[0] = axis1;
m_axis[1] = axis2;
m_limit[0] = m_limit[1] = m_limit[2] = false;
m_limit[0] = m_limit[1] = m_limit[2] = false;
}
IK_QTranslateSegment::IK_QTranslateSegment()
: IK_QSegment(3, true)
IK_QTranslateSegment::IK_QTranslateSegment() : IK_QSegment(3, true)
{
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
m_axis[0] = 0;
m_axis[1] = 1;
m_axis[2] = 2;
m_axis[0] = 0;
m_axis[1] = 1;
m_axis[2] = 2;
m_limit[0] = m_limit[1] = m_limit[2] = false;
m_limit[0] = m_limit[1] = m_limit[2] = false;
}
Vector3d IK_QTranslateSegment::Axis(int dof) const
{
return m_global_transform.linear().col(m_axis[dof]);
return m_global_transform.linear().col(m_axis[dof]);
}
bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp)
bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
{
int dof_id = m_DoF_id, dof = 0, i, clamped = false;
int dof_id = m_DoF_id, dof = 0, i, clamped = false;
Vector3d dx(0.0, 0.0, 0.0);
Vector3d dx(0.0, 0.0, 0.0);
for (i = 0; i < 3; i++) {
if (!m_axis_enabled[i]) {
m_new_translation[i] = m_translation[i];
continue;
}
for (i = 0; i < 3; i++) {
if (!m_axis_enabled[i]) {
m_new_translation[i] = m_translation[i];
continue;
}
clamp[dof] = false;
clamp[dof] = false;
if (!m_locked[dof]) {
m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
if (!m_locked[dof]) {
m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
if (m_limit[i]) {
if (m_new_translation[i] > m_max[i]) {
delta[dof] = m_max[i] - m_translation[i];
m_new_translation[i] = m_max[i];
clamped = clamp[dof] = true;
}
else if (m_new_translation[i] < m_min[i]) {
delta[dof] = m_min[i] - m_translation[i];
m_new_translation[i] = m_min[i];
clamped = clamp[dof] = true;
}
}
}
if (m_limit[i]) {
if (m_new_translation[i] > m_max[i]) {
delta[dof] = m_max[i] - m_translation[i];
m_new_translation[i] = m_max[i];
clamped = clamp[dof] = true;
}
else if (m_new_translation[i] < m_min[i]) {
delta[dof] = m_min[i] - m_translation[i];
m_new_translation[i] = m_min[i];
clamped = clamp[dof] = true;
}
}
}
dof_id++;
dof++;
}
dof_id++;
dof++;
}
return clamped;
return clamped;
}
void IK_QTranslateSegment::UpdateAngleApply()
{
m_translation = m_new_translation;
m_translation = m_new_translation;
}
void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta)
void IK_QTranslateSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
{
m_locked[dof] = true;
jacobian.Lock(m_DoF_id + dof, delta[dof]);
m_locked[dof] = true;
jacobian.Lock(m_DoF_id + dof, delta[dof]);
}
void IK_QTranslateSegment::SetWeight(int axis, double weight)
{
int i;
int i;
for (i = 0; i < m_num_DoF; i++)
if (m_axis[i] == axis)
m_weight[i] = weight;
for (i = 0; i < m_num_DoF; i++)
if (m_axis[i] == axis)
m_weight[i] = weight;
}
void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax)
{
if (lmax < lmin)
return;
if (lmax < lmin)
return;
m_min[axis] = lmin;
m_max[axis] = lmax;
m_limit[axis] = true;
m_min[axis] = lmin;
m_max[axis] = lmax;
m_limit[axis] = true;
}
void IK_QTranslateSegment::Scale(double scale)
{
int i;
int i;
IK_QSegment::Scale(scale);
IK_QSegment::Scale(scale);
for (i = 0; i < 3; i++) {
m_min[0] *= scale;
m_max[1] *= scale;
}
for (i = 0; i < 3; i++) {
m_min[0] *= scale;
m_max[1] *= scale;
}
m_new_translation *= scale;
m_new_translation *= scale;
}

459
intern/iksolver/intern/IK_QSegment.h
View File

@@ -49,289 +49,330 @@
* use exactly the same transformations when displaying the segments
*/
class IK_QSegment
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual ~IK_QSegment();
class IK_QSegment {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual ~IK_QSegment();
// start: a user defined translation
// rest_basis: a user defined rotation
// basis: a user defined rotation
// length: length of this segment
// start: a user defined translation
// rest_basis: a user defined rotation
// basis: a user defined rotation
// length: length of this segment
void SetTransform(
const Vector3d& start,
const Matrix3d& rest_basis,
const Matrix3d& basis,
const double length
);
void SetTransform(const Vector3d &start,
const Matrix3d &rest_basis,
const Matrix3d &basis,
const double length);
// tree structure access
void SetParent(IK_QSegment *parent);
// tree structure access
void SetParent(IK_QSegment *parent);
IK_QSegment *Child() const
{ return m_child; }
IK_QSegment *Child() const
{
return m_child;
}
IK_QSegment *Sibling() const
{ return m_sibling; }
IK_QSegment *Sibling() const
{
return m_sibling;
}
IK_QSegment *Parent() const
{ return m_parent; }
IK_QSegment *Parent() const
{
return m_parent;
}
// for combining two joints into one from the interface
void SetComposite(IK_QSegment *seg);
IK_QSegment *Composite() const
{ return m_composite; }
// for combining two joints into one from the interface
void SetComposite(IK_QSegment *seg);
// number of degrees of freedom
int NumberOfDoF() const
{ return m_num_DoF; }
IK_QSegment *Composite() const
{
return m_composite;
}
// unique id for this segment, for identification in the jacobian
int DoFId() const
{ return m_DoF_id; }
// number of degrees of freedom
int NumberOfDoF() const
{
return m_num_DoF;
}
void SetDoFId(int dof_id)
{ m_DoF_id = dof_id; }
// unique id for this segment, for identification in the jacobian
int DoFId() const
{
return m_DoF_id;
}
// the max distance of the end of this bone from the local origin.
const double MaxExtension() const
{ return m_max_extension; }
void SetDoFId(int dof_id)
{
m_DoF_id = dof_id;
}
// the change in rotation and translation w.r.t. the rest pose
Matrix3d BasisChange() const;
Vector3d TranslationChange() const;
// the max distance of the end of this bone from the local origin.
const double MaxExtension() const
{
return m_max_extension;
}
// the start and end of the segment
const Vector3d GlobalStart() const
{ return m_global_start; }
// the change in rotation and translation w.r.t. the rest pose
Matrix3d BasisChange() const;
Vector3d TranslationChange() const;
const Vector3d GlobalEnd() const
{ return m_global_transform.translation(); }
// the start and end of the segment
const Vector3d GlobalStart() const
{
return m_global_start;
}
// the global transformation at the end of the segment
const Affine3d &GlobalTransform() const
{ return m_global_transform; }
const Vector3d GlobalEnd() const
{
return m_global_transform.translation();
}
// is a translational segment?
bool Translational() const
{ return m_translational; }
// the global transformation at the end of the segment
const Affine3d &GlobalTransform() const
{
return m_global_transform;
}
// locking (during inner clamping loop)
bool Locked(int dof) const
{ return m_locked[dof]; }
// is a translational segment?
bool Translational() const
{
return m_translational;
}
void UnLock()
{ m_locked[0] = m_locked[1] = m_locked[2] = false; }
// locking (during inner clamping loop)
bool Locked(int dof) const
{
return m_locked[dof];
}
// per dof joint weighting
double Weight(int dof) const
{ return m_weight[dof]; }
void UnLock()
{
m_locked[0] = m_locked[1] = m_locked[2] = false;
}
void ScaleWeight(int dof, double scale)
{ m_weight[dof] *= scale; }
// per dof joint weighting
double Weight(int dof) const
{
return m_weight[dof];
}
// recursively update the global coordinates of this segment, 'global'
// is the global transformation from the parent segment
void UpdateTransform(const Affine3d &global);
void ScaleWeight(int dof, double scale)
{
m_weight[dof] *= scale;
}
// get axis from rotation matrix for derivative computation
virtual Vector3d Axis(int dof) const=0;
// recursively update the global coordinates of this segment, 'global'
// is the global transformation from the parent segment
void UpdateTransform(const Affine3d &global);
// update the angles using the dTheta's computed using the jacobian matrix
virtual bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*)=0;
virtual void Lock(int, IK_QJacobian&, Vector3d&) {}
virtual void UpdateAngleApply()=0;
// get axis from rotation matrix for derivative computation
virtual Vector3d Axis(int dof) const = 0;
// set joint limits
virtual void SetLimit(int, double, double) {}
// update the angles using the dTheta's computed using the jacobian matrix
virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *) = 0;
virtual void Lock(int, IK_QJacobian &, Vector3d &)
{
}
virtual void UpdateAngleApply() = 0;
// set joint weights (per axis)
virtual void SetWeight(int, double) {}
// set joint limits
virtual void SetLimit(int, double, double)
{
}
virtual void SetBasis(const Matrix3d& basis) { m_basis = basis; }
// set joint weights (per axis)
virtual void SetWeight(int, double)
{
}
// functions needed for pole vector constraint
void PrependBasis(const Matrix3d& mat);
void Reset();
virtual void SetBasis(const Matrix3d &basis)
{
m_basis = basis;
}
// scale
virtual void Scale(double scale);
// functions needed for pole vector constraint
void PrependBasis(const Matrix3d &mat);
void Reset();
protected:
// scale
virtual void Scale(double scale);
// num_DoF: number of degrees of freedom
IK_QSegment(int num_DoF, bool translational);
protected:
// num_DoF: number of degrees of freedom
IK_QSegment(int num_DoF, bool translational);
// remove child as a child of this segment
void RemoveChild(IK_QSegment *child);
// remove child as a child of this segment
void RemoveChild(IK_QSegment *child);
// tree structure variables
IK_QSegment *m_parent;
IK_QSegment *m_child;
IK_QSegment *m_sibling;
IK_QSegment *m_composite;
// tree structure variables
IK_QSegment *m_parent;
IK_QSegment *m_child;
IK_QSegment *m_sibling;
IK_QSegment *m_composite;
// full transform =
// start * rest_basis * basis * translation
Vector3d m_start;
Matrix3d m_rest_basis;
Matrix3d m_basis;
Vector3d m_translation;
// full transform =
// start * rest_basis * basis * translation
Vector3d m_start;
Matrix3d m_rest_basis;
Matrix3d m_basis;
Vector3d m_translation;
// original basis
Matrix3d m_orig_basis;
Vector3d m_orig_translation;
// original basis
Matrix3d m_orig_basis;
Vector3d m_orig_translation;
// maximum extension of this segment
double m_max_extension;
// maximum extension of this segment
double m_max_extension;
// accumulated transformations starting from root
Vector3d m_global_start;
Affine3d m_global_transform;
// accumulated transformations starting from root
Vector3d m_global_start;
Affine3d m_global_transform;
// number degrees of freedom, (first) id of this segments DOF's
int m_num_DoF, m_DoF_id;
// number degrees of freedom, (first) id of this segments DOF's
int m_num_DoF, m_DoF_id;
bool m_locked[3];
bool m_translational;
double m_weight[3];
bool m_locked[3];
bool m_translational;
double m_weight[3];
};
class IK_QSphericalSegment : public IK_QSegment
{
public:
IK_QSphericalSegment();
class IK_QSphericalSegment : public IK_QSegment {
public:
IK_QSphericalSegment();
Vector3d Axis(int dof) const;
Vector3d Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
void UpdateAngleApply();
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
void UpdateAngleApply();
bool ComputeClampRotation(Vector3d& clamp);
bool ComputeClampRotation(Vector3d &clamp);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
private:
Matrix3d m_new_basis;
bool m_limit_x, m_limit_y, m_limit_z;
double m_min[2], m_max[2];
double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
double m_locked_ax, m_locked_ay, m_locked_az;
private:
Matrix3d m_new_basis;
bool m_limit_x, m_limit_y, m_limit_z;
double m_min[2], m_max[2];
double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
double m_locked_ax, m_locked_ay, m_locked_az;
};
class IK_QNullSegment : public IK_QSegment
{
public:
IK_QNullSegment();
class IK_QNullSegment : public IK_QSegment {
public:
IK_QNullSegment();
bool UpdateAngle(const IK_QJacobian&, Vector3d&, bool*) { return false; }
void UpdateAngleApply() {}
bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *)
{
return false;
}
void UpdateAngleApply()
{
}
Vector3d Axis(int) const { return Vector3d(0, 0, 0); }
void SetBasis(const Matrix3d&) { m_basis.setIdentity(); }
Vector3d Axis(int) const
{
return Vector3d(0, 0, 0);
}
void SetBasis(const Matrix3d &)
{
m_basis.setIdentity();
}
};
class IK_QRevoluteSegment : public IK_QSegment
{
public:
// axis: the axis of the DoF, in range 0..2
IK_QRevoluteSegment(int axis);
class IK_QRevoluteSegment : public IK_QSegment {
public:
// axis: the axis of the DoF, in range 0..2
IK_QRevoluteSegment(int axis);
Vector3d Axis(int dof) const;
Vector3d Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
void UpdateAngleApply();
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
void UpdateAngleApply();
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d& basis);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d &basis);
private:
int m_axis;
double m_angle, m_new_angle;
bool m_limit;
double m_min, m_max;
private:
int m_axis;
double m_angle, m_new_angle;
bool m_limit;
double m_min, m_max;
};
class IK_QSwingSegment : public IK_QSegment
{
public:
// XZ DOF, uses one direct rotation
IK_QSwingSegment();
class IK_QSwingSegment : public IK_QSegment {
public:
// XZ DOF, uses one direct rotation
IK_QSwingSegment();
Vector3d Axis(int dof) const;
Vector3d Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
void UpdateAngleApply();
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
void UpdateAngleApply();
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d& basis);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d &basis);
private:
Matrix3d m_new_basis;
bool m_limit_x, m_limit_z;
double m_min[2], m_max[2];
double m_max_x, m_max_z, m_offset_x, m_offset_z;
private:
Matrix3d m_new_basis;
bool m_limit_x, m_limit_z;
double m_min[2], m_max[2];
double m_max_x, m_max_z, m_offset_x, m_offset_z;
};
class IK_QElbowSegment : public IK_QSegment
{
public:
// XY or ZY DOF, uses two sequential rotations: first rotate around
// X or Z, then rotate around Y (twist)
IK_QElbowSegment(int axis);
class IK_QElbowSegment : public IK_QSegment {
public:
// XY or ZY DOF, uses two sequential rotations: first rotate around
// X or Z, then rotate around Y (twist)
IK_QElbowSegment(int axis);
Vector3d Axis(int dof) const;
Vector3d Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, Vector3d& delta);
void UpdateAngleApply();
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta);
void UpdateAngleApply();
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d& basis);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetBasis(const Matrix3d &basis);
private:
int m_axis;
private:
int m_axis;
double m_twist, m_angle, m_new_twist, m_new_angle;
double m_cos_twist, m_sin_twist;
double m_twist, m_angle, m_new_twist, m_new_angle;
double m_cos_twist, m_sin_twist;
bool m_limit, m_limit_twist;
double m_min, m_max, m_min_twist, m_max_twist;
bool m_limit, m_limit_twist;
double m_min, m_max, m_min_twist, m_max_twist;
};
class IK_QTranslateSegment : public IK_QSegment
{
public:
// 1DOF, 2DOF or 3DOF translational segments
IK_QTranslateSegment(int axis1);
IK_QTranslateSegment(int axis1, int axis2);
IK_QTranslateSegment();
class IK_QTranslateSegment : public IK_QSegment {
public:
// 1DOF, 2DOF or 3DOF translational segments
IK_QTranslateSegment(int axis1);
IK_QTranslateSegment(int axis1, int axis2);
IK_QTranslateSegment();
Vector3d Axis(int dof) const;
Vector3d Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d& delta, bool *clamp);
void Lock(int, IK_QJacobian&, Vector3d&);
void UpdateAngleApply();
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp);
void Lock(int, IK_QJacobian &, Vector3d &);
void UpdateAngleApply();
void SetWeight(int axis, double weight);
void SetLimit(int axis, double lmin, double lmax);
void SetWeight(int axis, double weight);
void SetLimit(int axis, double lmin, double lmax);
void Scale(double scale);
void Scale(double scale);
private:
int m_axis[3];
bool m_axis_enabled[3], m_limit[3];
Vector3d m_new_translation;
double m_min[3], m_max[3];
private:
int m_axis[3];
bool m_axis_enabled[3], m_limit[3];
Vector3d m_new_translation;
double m_min[3], m_max[3];
};

250
intern/iksolver/intern/IK_QTask.cpp
View File

@@ -21,210 +21,196 @@
* \ingroup 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_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)
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;
// computing clamping length
int num;
const IK_QSegment *seg;
m_clamp_length = 0.0;
num = 0;
m_clamp_length = 0.0;
num = 0;
for (seg = m_segment; seg; seg = seg->Parent()) {
m_clamp_length += seg->MaxExtension();
num++;
}
for (seg = m_segment; seg; seg = seg->Parent()) {
m_clamp_length += seg->MaxExtension();
num++;
}
m_clamp_length /= 2 * num;
m_clamp_length /= 2 * num;
}
void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian)
{
// compute beta
const Vector3d& pos = m_segment->GlobalEnd();
// compute beta
const Vector3d &pos = m_segment->GlobalEnd();
Vector3d d_pos = m_goal - pos;
double length = d_pos.norm();
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);
if (length > m_clamp_length)
d_pos = (m_clamp_length / length) * d_pos;
// compute derivatives
int i;
const IK_QSegment *seg;
jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
for (seg = m_segment; seg; seg = seg->Parent()) {
Vector3d p = seg->GlobalStart() - pos;
// compute derivatives
int i;
const IK_QSegment *seg;
for (i = 0; i < seg->NumberOfDoF(); i++) {
Vector3d axis = seg->Axis(i) * m_weight;
for (seg = m_segment; seg; seg = seg->Parent()) {
Vector3d p = seg->GlobalStart() - pos;
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);
}
}
}
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();
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)
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)
void IK_QOrientationTask::ComputeJacobian(IK_QJacobian &jacobian)
{
// compute betas
const Matrix3d& rot = m_segment->GlobalTransform().linear();
// compute betas
const Matrix3d &rot = m_segment->GlobalTransform().linear();
Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
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));
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();
m_distance = d_rot.norm();
jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
// compute derivatives
int i;
const IK_QSegment *seg;
// compute derivatives
int i;
const IK_QSegment *seg;
for (seg = m_segment; seg; seg = seg->Parent())
for (i = 0; i < seg->NumberOfDoF(); i++) {
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);
}
}
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)
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;
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;
double mass = /*seg->Mass()*/ 1.0;
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
mass += ComputeTotalMass(seg);
return mass;
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();
Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
const IK_QSegment *seg;
for (seg = segment->Child(); seg; seg = seg->Sibling())
center += ComputeCenter(seg);
return center;
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& center, const IK_QSegment *segment)
void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
Vector3d &center,
const IK_QSegment *segment)
{
int i;
Vector3d p = center - segment->GlobalStart();
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);
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)
void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian &jacobian)
{
Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
// compute beta
Vector3d d_pos = m_goal_center - center;
// compute beta
Vector3d d_pos = m_goal_center - center;
m_distance = d_pos.norm();
m_distance = d_pos.norm();
#if 0
if (m_distance > m_clamp_length)
d_pos = (m_clamp_length / m_distance) * d_pos;
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);
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;
return m_distance;
}

178
intern/iksolver/intern/IK_QTask.h
View File

@@ -28,116 +28,128 @@
#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() {}
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; }
int Id() const
{
return m_size;
}
void SetId(int id)
{ m_id = id; }
void SetId(int id)
{
m_id = id;
}
int Size() const
{ return m_size; }
int Size() const
{
return m_size;
}
bool Primary() const
{ return m_primary; }
bool Primary() const
{
return m_primary;
}
bool Active() const
{ return m_active; }
bool Active() const
{
return m_active;
}
double Weight() const
{ return m_weight*m_weight; }
double Weight() const
{
return m_weight * m_weight;
}
void SetWeight(double weight)
{ m_weight = sqrt(weight); }
void SetWeight(double weight)
{
m_weight = sqrt(weight);
}
virtual void ComputeJacobian(IK_QJacobian& jacobian)=0;
virtual void ComputeJacobian(IK_QJacobian &jacobian) = 0;
virtual double Distance() const=0;
virtual double Distance() const = 0;
virtual bool PositionTask() const { return false; }
virtual bool PositionTask() const
{
return false;
}
virtual void Scale(double) {}
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;
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
);
class IK_QPositionTask : public IK_QTask {
public:
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal);
void ComputeJacobian(IK_QJacobian& jacobian);
void ComputeJacobian(IK_QJacobian &jacobian);
double Distance() const;
double Distance() const;
bool PositionTask() const { return true; }
void Scale(double scale) { m_goal *= scale; m_clamp_length *= scale; }
bool PositionTask() const
{
return true;
}
void Scale(double scale)
{
m_goal *= scale;
m_clamp_length *= scale;
}
private:
Vector3d m_goal;
double m_clamp_length;
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
);
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);
double Distance() const
{
return m_distance;
}
void ComputeJacobian(IK_QJacobian &jacobian);
private:
Matrix3d m_goal;
double m_distance;
private:
Matrix3d m_goal;
double m_distance;
};
class IK_QCenterOfMassTask : public IK_QTask {
public:
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d &center);
class IK_QCenterOfMassTask : public IK_QTask
{
public:
IK_QCenterOfMassTask(
bool primary,
const IK_QSegment *segment,
const Vector3d& center
);
void ComputeJacobian(IK_QJacobian &jacobian);
void ComputeJacobian(IK_QJacobian& jacobian);
double Distance() const;
double Distance() const;
void Scale(double scale)
{
m_goal_center *= scale;
m_distance *= scale;
}
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 &center, const IK_QSegment *segment);
private:
double ComputeTotalMass(const IK_QSegment *segment);
Vector3d ComputeCenter(const IK_QSegment *segment);
void JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment);
Vector3d m_goal_center;
double m_total_mass_inv;
double m_distance;
Vector3d m_goal_center;
double m_total_mass_inv;
double m_distance;
};

491
intern/iksolver/intern/IK_Solver.cpp
View File

@@ -21,7 +21,6 @@
* \ingroup iksolver
*/
#include "../extern/IK_solver.h"
#include "IK_QJacobianSolver.h"
@@ -32,355 +31,387 @@
using namespace std;
class IK_QSolver {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
IK_QSolver() : root(NULL) {
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
IK_QSolver() : root(NULL)
{
}
IK_QJacobianSolver solver;
IK_QSegment *root;
std::list<IK_QTask *> tasks;
IK_QJacobianSolver solver;
IK_QSegment *root;
std::list<IK_QTask *> tasks;
};
// FIXME: locks still result in small "residual" changes to the locked axes...
static IK_QSegment *CreateSegment(int flag, bool translate)
{
int ndof = 0;
ndof += (flag & IK_XDOF) ? 1 : 0;
ndof += (flag & IK_YDOF) ? 1 : 0;
ndof += (flag & IK_ZDOF) ? 1 : 0;
int ndof = 0;
ndof += (flag & IK_XDOF) ? 1 : 0;
ndof += (flag & IK_YDOF) ? 1 : 0;
ndof += (flag & IK_ZDOF) ? 1 : 0;
IK_QSegment *seg;
IK_QSegment *seg;
if (ndof == 0)
return NULL;
else if (ndof == 1) {
int axis;
if (ndof == 0)
return NULL;
else if (ndof == 1) {
int axis;
if (flag & IK_XDOF) axis = 0;
else if (flag & IK_YDOF) axis = 1;
else axis = 2;
if (flag & IK_XDOF)
axis = 0;
else if (flag & IK_YDOF)
axis = 1;
else
axis = 2;
if (translate)
seg = new IK_QTranslateSegment(axis);
else
seg = new IK_QRevoluteSegment(axis);
}
else if (ndof == 2) {
int axis1, axis2;
if (translate)
seg = new IK_QTranslateSegment(axis);
else
seg = new IK_QRevoluteSegment(axis);
}
else if (ndof == 2) {
int axis1, axis2;
if (flag & IK_XDOF) {
axis1 = 0;
axis2 = (flag & IK_YDOF) ? 1 : 2;
}
else {
axis1 = 1;
axis2 = 2;
}
if (flag & IK_XDOF) {
axis1 = 0;
axis2 = (flag & IK_YDOF) ? 1 : 2;
}
else {
axis1 = 1;
axis2 = 2;
}
if (translate)
seg = new IK_QTranslateSegment(axis1, axis2);
else {
if (axis1 + axis2 == 2)
seg = new IK_QSwingSegment();
else
seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
}
}
else {
if (translate)
seg = new IK_QTranslateSegment();
else
seg = new IK_QSphericalSegment();
}
if (translate)
seg = new IK_QTranslateSegment(axis1, axis2);
else {
if (axis1 + axis2 == 2)
seg = new IK_QSwingSegment();
else
seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
}
}
else {
if (translate)
seg = new IK_QTranslateSegment();
else
seg = new IK_QSphericalSegment();
}
return seg;
return seg;
}
IK_Segment *IK_CreateSegment(int flag)
{
IK_QSegment *rot = CreateSegment(flag, false);
IK_QSegment *trans = CreateSegment(flag >> 3, true);
IK_QSegment *rot = CreateSegment(flag, false);
IK_QSegment *trans = CreateSegment(flag >> 3, true);
IK_QSegment *seg;
IK_QSegment *seg;
if (rot == NULL && trans == NULL)
seg = new IK_QNullSegment();
else if (rot == NULL)
seg = trans;
else {
seg = rot;
if (rot == NULL && trans == NULL)
seg = new IK_QNullSegment();
else if (rot == NULL)
seg = trans;
else {
seg = rot;
// make it seem from the interface as if the rotation and translation
// segment are one
if (trans) {
seg->SetComposite(trans);
trans->SetParent(seg);
}
}
// make it seem from the interface as if the rotation and translation
// segment are one
if (trans) {
seg->SetComposite(trans);
trans->SetParent(seg);
}
}
return seg;
return seg;
}
void IK_FreeSegment(IK_Segment *seg)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Composite())
delete qseg->Composite();
delete qseg;
if (qseg->Composite())
delete qseg->Composite();
delete qseg;
}
void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qparent = (IK_QSegment *)parent;
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qparent = (IK_QSegment *)parent;
if (qparent && qparent->Composite())
qseg->SetParent(qparent->Composite());
else
qseg->SetParent(qparent);
if (qparent && qparent->Composite())
qseg->SetParent(qparent->Composite());
else
qseg->SetParent(qparent);
}
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
void IK_SetTransform(
IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
Vector3d mstart(start[0], start[1], start[2]);
// convert from blender column major
Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
double mlength(length);
Vector3d mstart(start[0], start[1], start[2]);
// convert from blender column major
Matrix3d mbasis = CreateMatrix(basis[0][0],
basis[1][0],
basis[2][0],
basis[0][1],
basis[1][1],
basis[2][1],
basis[0][2],
basis[1][2],
basis[2][2]);
Matrix3d mrest = CreateMatrix(rest[0][0],
rest[1][0],
rest[2][0],
rest[0][1],
rest[1][1],
rest[2][1],
rest[0][2],
rest[1][2],
rest[2][2]);
double mlength(length);
if (qseg->Composite()) {
Vector3d cstart(0, 0, 0);
Matrix3d cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
if (qseg->Composite()) {
Vector3d cstart(0, 0, 0);
Matrix3d cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
if (axis == IK_TRANS_X) axis = IK_X;
else if (axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
if (axis == IK_TRANS_X)
axis = IK_X;
else if (axis == IK_TRANS_Y)
axis = IK_Y;
else
axis = IK_Z;
}
qseg->SetLimit(axis, lmin, lmax);
qseg->SetLimit(axis, lmin, lmax);
}
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
if (stiffness < 0.0f)
return;
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS))
stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
if (stiffness < 0.0f)
return;
IK_QSegment *qseg = (IK_QSegment *)seg;
double weight = 1.0f - stiffness;
if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS))
stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
IK_QSegment *qseg = (IK_QSegment *)seg;
double weight = 1.0f - stiffness;
if (axis == IK_TRANS_X) axis = IK_X;
else if (axis == IK_TRANS_Y) axis = IK_Y;
else axis = IK_Z;
}
if (axis >= IK_TRANS_X) {
if (!qseg->Translational()) {
if (qseg->Composite() && qseg->Composite()->Translational())
qseg = qseg->Composite();
else
return;
}
qseg->SetWeight(axis, weight);
if (axis == IK_TRANS_X)
axis = IK_X;
else if (axis == IK_TRANS_Y)
axis = IK_Y;
else
axis = IK_Z;
}
qseg->SetWeight(axis, weight);
}
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
if (qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
const Matrix3d& change = qseg->BasisChange();
const Matrix3d &change = qseg->BasisChange();
// convert to blender column major
basis_change[0][0] = (float)change(0, 0);
basis_change[1][0] = (float)change(0, 1);
basis_change[2][0] = (float)change(0, 2);
basis_change[0][1] = (float)change(1, 0);
basis_change[1][1] = (float)change(1, 1);
basis_change[2][1] = (float)change(1, 2);
basis_change[0][2] = (float)change(2, 0);
basis_change[1][2] = (float)change(2, 1);
basis_change[2][2] = (float)change(2, 2);
// convert to blender column major
basis_change[0][0] = (float)change(0, 0);
basis_change[1][0] = (float)change(0, 1);
basis_change[2][0] = (float)change(0, 2);
basis_change[0][1] = (float)change(1, 0);
basis_change[1][1] = (float)change(1, 1);
basis_change[2][1] = (float)change(1, 2);
basis_change[0][2] = (float)change(2, 0);
basis_change[1][2] = (float)change(2, 1);
basis_change[2][2] = (float)change(2, 2);
}
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
IK_QSegment *qseg = (IK_QSegment *)seg;
if (!qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
const Vector3d& change = qseg->TranslationChange();
if (!qseg->Translational() && qseg->Composite())
qseg = qseg->Composite();
translation_change[0] = (float)change[0];
translation_change[1] = (float)change[1];
translation_change[2] = (float)change[2];
const Vector3d &change = qseg->TranslationChange();
translation_change[0] = (float)change[0];
translation_change[1] = (float)change[1];
translation_change[2] = (float)change[2];
}
IK_Solver *IK_CreateSolver(IK_Segment *root)
{
if (root == NULL)
return NULL;
IK_QSolver *solver = new IK_QSolver();
solver->root = (IK_QSegment *)root;
if (root == NULL)
return NULL;
return (IK_Solver *)solver;
IK_QSolver *solver = new IK_QSolver();
solver->root = (IK_QSegment *)root;
return (IK_Solver *)solver;
}
void IK_FreeSolver(IK_Solver *solver)
{
if (solver == NULL)
return;
if (solver == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver *)solver;
std::list<IK_QTask *>& tasks = qsolver->tasks;
std::list<IK_QTask *>::iterator task;
IK_QSolver *qsolver = (IK_QSolver *)solver;
std::list<IK_QTask *> &tasks = qsolver->tasks;
std::list<IK_QTask *>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++)
delete (*task);
delete qsolver;
for (task = tasks.begin(); task != tasks.end(); task++)
delete (*task);
delete qsolver;
}
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
if (solver == NULL || tip == NULL)
return;
if (solver == NULL || tip == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
Vector3d pos(goal[0], goal[1], goal[2]);
Vector3d pos(goal[0], goal[1], goal[2]);
IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
ee->SetWeight(weight);
qsolver->tasks.push_back(ee);
IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
ee->SetWeight(weight);
qsolver->tasks.push_back(ee);
}
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
if (solver == NULL || tip == NULL)
return;
if (solver == NULL || tip == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
// convert from blender column major
Matrix3d rot = CreateMatrix(goal[0][0], goal[1][0], goal[2][0],
goal[0][1], goal[1][1], goal[2][1],
goal[0][2], goal[1][2], goal[2][2]);
// convert from blender column major
Matrix3d rot = CreateMatrix(goal[0][0],
goal[1][0],
goal[2][0],
goal[0][1],
goal[1][1],
goal[2][1],
goal[0][2],
goal[1][2],
goal[2][2]);
IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
orient->SetWeight(weight);
qsolver->tasks.push_back(orient);
IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
orient->SetWeight(weight);
qsolver->tasks.push_back(orient);
}
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
IK_Segment *tip,
float goal[3],
float polegoal[3],
float poleangle,
int getangle)
{
if (solver == NULL || tip == NULL)
return;
if (solver == NULL || tip == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qtip = (IK_QSegment *)tip;
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
// in case of composite segment the second segment is the tip
if (qtip->Composite())
qtip = qtip->Composite();
Vector3d qgoal(goal[0], goal[1], goal[2]);
Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
Vector3d qgoal(goal[0], goal[1], goal[2]);
Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
qsolver->solver.SetPoleVectorConstraint(
qtip, qgoal, qpolegoal, poleangle, getangle);
qsolver->solver.SetPoleVectorConstraint(qtip, qgoal, qpolegoal, poleangle, getangle);
}
float IK_SolverGetPoleAngle(IK_Solver *solver)
{
if (solver == NULL)
return 0.0f;
if (solver == NULL)
return 0.0f;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSolver *qsolver = (IK_QSolver *)solver;
return qsolver->solver.GetPoleAngle();
return qsolver->solver.GetPoleAngle();
}
#if 0
static void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
{
if (solver == NULL || root == NULL)
return;
if (solver == NULL || root == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qroot = (IK_QSegment *)root;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *qroot = (IK_QSegment *)root;
// convert from blender column major
Vector3d center(goal);
// convert from blender column major
Vector3d center(goal);
IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
com->SetWeight(weight);
qsolver->tasks.push_back(com);
IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
com->SetWeight(weight);
qsolver->tasks.push_back(com);
}
#endif
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
{
if (solver == NULL)
return 0;
if (solver == NULL)
return 0;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSolver *qsolver = (IK_QSolver *)solver;
IK_QSegment *root = qsolver->root;
IK_QJacobianSolver& jacobian = qsolver->solver;
std::list<IK_QTask *>& tasks = qsolver->tasks;
double tol = tolerance;
IK_QSegment *root = qsolver->root;
IK_QJacobianSolver &jacobian = qsolver->solver;
std::list<IK_QTask *> &tasks = qsolver->tasks;
double tol = tolerance;
if (!jacobian.Setup(root, tasks))
return 0;
if (!jacobian.Setup(root, tasks))
return 0;
bool result = jacobian.Solve(root, tasks, tol, max_iterations);
bool result = jacobian.Solve(root, tasks, tol, max_iterations);
return ((result) ? 1 : 0);
return ((result) ? 1 : 0);
}