Got rid of unused files in intern/iksolver
(removed them from cvs and from the Makefile.am) Kent -- mein@cs.umn.edu
This commit is contained in:
Kent Mein
parent
5b88406c3f
commit
671a355e9f
|
|
@ -1,303 +0,0 @@
|
|||
/**
|
||||
* $Id$
|
||||
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* as published by the Free Software Foundation; either version 2
|
||||
* of the License, or (at your option) any later version. The Blender
|
||||
* Foundation also sells licenses for use in proprietary software under
|
||||
* the Blender License. See http://www.blender.org/BL/ for information
|
||||
* about this.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software Foundation,
|
||||
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
*
|
||||
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
||||
* All rights reserved.
|
||||
*
|
||||
* The Original Code is: all of this file.
|
||||
*
|
||||
* Contributor(s): none yet.
|
||||
*
|
||||
* ***** END GPL/BL DUAL LICENSE BLOCK *****
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include "IK_CGChainSolver.h"
|
||||
#include "IK_Segment.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
ChainPotential *
|
||||
ChainPotential::
|
||||
New(
|
||||
IK_Chain &chain
|
||||
){
|
||||
return new ChainPotential(chain);
|
||||
};
|
||||
|
||||
|
||||
|
||||
// End effector goal
|
||||
|
||||
void
|
||||
ChainPotential::
|
||||
SetGoal(
|
||||
const MT_Vector3 goal
|
||||
){
|
||||
m_goal = goal;
|
||||
};
|
||||
|
||||
// Inherited from DifferentiablePotenialFunctionNd
|
||||
//////////////////////////////////////////////////
|
||||
|
||||
MT_Scalar
|
||||
ChainPotential::
|
||||
Evaluate(
|
||||
MT_Scalar x
|
||||
){
|
||||
|
||||
// evaluate the function at postiion x along the line specified
|
||||
// by the vector pair (m_line_pos,m_line_dir)
|
||||
|
||||
m_temp_pos.newsize(m_line_dir.size());
|
||||
|
||||
TNT::vectorscale(m_temp_pos,m_line_dir,x);
|
||||
TNT::vectoradd(m_temp_pos,m_line_pos);
|
||||
|
||||
return Evaluate(m_temp_pos);
|
||||
}
|
||||
|
||||
MT_Scalar
|
||||
ChainPotential::
|
||||
Derivative(
|
||||
MT_Scalar x
|
||||
){
|
||||
m_temp_pos.newsize(m_line_dir.size());
|
||||
m_temp_grad.newsize(m_line_dir.size());
|
||||
|
||||
TNT::vectorscale(m_temp_pos,m_line_dir,x);
|
||||
TNT::vectoradd(m_temp_pos,m_line_pos);
|
||||
|
||||
Derivative(m_temp_pos,m_temp_grad);
|
||||
|
||||
return TNT::dot_prod(m_temp_grad,m_line_dir);
|
||||
}
|
||||
|
||||
|
||||
MT_Scalar
|
||||
ChainPotential::
|
||||
Evaluate(
|
||||
const TNT::Vector<MT_Scalar> &x
|
||||
){
|
||||
|
||||
|
||||
// set the vector of angles in the backup chain
|
||||
|
||||
vector<IK_Segment> &segs = m_t_chain.Segments();
|
||||
vector<IK_Segment>::iterator seg_it = segs.begin();
|
||||
TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
|
||||
#if 0
|
||||
TNT::Vector<MT_Scalar>::const_iterator a_end = x.end();
|
||||
#endif
|
||||
// while we are iterating through the angles and segments
|
||||
// we compute the current angle potenial
|
||||
MT_Scalar angle_potential = 0;
|
||||
#if 0
|
||||
|
||||
for (; a_it != a_end; ++a_it, ++seg_it) {
|
||||
|
||||
MT_Scalar dif = (*a_it - seg_it->CentralAngle());
|
||||
dif *= dif * seg_it->Weight();
|
||||
angle_potential += dif;
|
||||
seg_it->SetTheta(*a_it);
|
||||
}
|
||||
#endif
|
||||
|
||||
int chain_dof = m_t_chain.DoF();
|
||||
int n;
|
||||
|
||||
for (n=0; n < chain_dof;seg_it ++) {
|
||||
n += seg_it->SetAngles(a_it + n);
|
||||
}
|
||||
|
||||
|
||||
m_t_chain.UpdateGlobalTransformations();
|
||||
|
||||
MT_Scalar output = (DistancePotential(m_t_chain.EndEffector(),m_goal) + angle_potential);
|
||||
|
||||
return output;
|
||||
};
|
||||
|
||||
void
|
||||
ChainPotential::
|
||||
Derivative(
|
||||
const TNT::Vector<MT_Scalar> &x,
|
||||
TNT::Vector<MT_Scalar> &dy
|
||||
){
|
||||
|
||||
m_distance_grad.newsize(3);
|
||||
// set the vector of angles in the backup chain
|
||||
|
||||
vector<IK_Segment> & segs = m_t_chain.Segments();
|
||||
vector<IK_Segment>::iterator seg_it = segs.begin();
|
||||
TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
|
||||
TNT::Vector<MT_Scalar>::const_iterator a_end = x.end();
|
||||
|
||||
m_angle_grad.newsize(segs.size());
|
||||
m_angle_grad = 0;
|
||||
#if 0
|
||||
// FIXME compute angle gradients
|
||||
TNT::Vector<MT_Scalar>::iterator ag_it = m_angle_grad.begin();
|
||||
#endif
|
||||
|
||||
const int chain_dof = m_t_chain.DoF();
|
||||
for (int n=0; n < chain_dof;seg_it ++) {
|
||||
n += seg_it->SetAngles(a_it + n);
|
||||
}
|
||||
|
||||
m_t_chain.UpdateGlobalTransformations();
|
||||
m_t_chain.ComputeJacobian();
|
||||
|
||||
DistanceGradient(m_t_chain.EndEffector(),m_goal);
|
||||
|
||||
// use chain rule for calculating derivative
|
||||
// of potenial function
|
||||
TNT::matmult(dy,m_t_chain.TransposedJacobian(),m_distance_grad);
|
||||
#if 0
|
||||
TNT::vectoradd(dy,m_angle_grad);
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
||||
MT_Scalar
|
||||
ChainPotential::
|
||||
DistancePotential(
|
||||
MT_Vector3 pos,
|
||||
MT_Vector3 goal
|
||||
) const {
|
||||
return (pos - goal).length2();
|
||||
}
|
||||
|
||||
// update m_distance_gradient
|
||||
|
||||
void
|
||||
ChainPotential::
|
||||
DistanceGradient(
|
||||
MT_Vector3 pos,
|
||||
MT_Vector3 goal
|
||||
){
|
||||
|
||||
MT_Vector3 output = 2*(pos - goal);
|
||||
m_distance_grad.newsize(3);
|
||||
|
||||
m_distance_grad[0] = output.x();
|
||||
m_distance_grad[1] = output.y();
|
||||
m_distance_grad[2] = output.z();
|
||||
}
|
||||
|
||||
|
||||
IK_CGChainSolver *
|
||||
IK_CGChainSolver::
|
||||
New(
|
||||
){
|
||||
|
||||
MEM_SmartPtr<IK_CGChainSolver> output (new IK_CGChainSolver());
|
||||
MEM_SmartPtr<IK_ConjugateGradientSolver>solver (IK_ConjugateGradientSolver::New());
|
||||
|
||||
if (output == NULL || solver == NULL) return NULL;
|
||||
|
||||
output->m_grad_solver = solver.Release();
|
||||
return output.Release();
|
||||
};
|
||||
|
||||
|
||||
bool
|
||||
IK_CGChainSolver::
|
||||
Solve(
|
||||
IK_Chain & chain,
|
||||
MT_Vector3 new_position,
|
||||
MT_Scalar tolerance
|
||||
){
|
||||
|
||||
// first build a potenial function for the chain
|
||||
|
||||
m_potential = ChainPotential::New(chain);
|
||||
if (m_potential == NULL) return false;
|
||||
|
||||
m_potential->SetGoal(new_position);
|
||||
|
||||
// make a TNT::vector to describe the current
|
||||
// chain state
|
||||
|
||||
TNT::Vector<MT_Scalar> p;
|
||||
p.newsize(chain.DoF());
|
||||
|
||||
TNT::Vector<MT_Scalar>::iterator p_it = p.begin();
|
||||
vector<IK_Segment>::const_iterator seg_it = chain.Segments().begin();
|
||||
vector<IK_Segment>::const_iterator seg_end = chain.Segments().end();
|
||||
|
||||
for (; seg_it != seg_end; seg_it++) {
|
||||
|
||||
int i;
|
||||
int seg_dof = seg_it->DoF();
|
||||
for (i=0; i < seg_dof; ++i,++p_it) {
|
||||
*p_it = seg_it->ActiveAngle(i);
|
||||
}
|
||||
}
|
||||
|
||||
// solve the thing
|
||||
int iterations(0);
|
||||
MT_Scalar return_value(0);
|
||||
|
||||
m_grad_solver->Solve(
|
||||
p,
|
||||
tolerance,
|
||||
iterations,
|
||||
return_value,
|
||||
m_potential.Ref(),
|
||||
100
|
||||
);
|
||||
|
||||
// update this chain
|
||||
|
||||
vector<IK_Segment>::iterator out_seg_it = chain.Segments().begin();
|
||||
TNT::Vector<MT_Scalar>::const_iterator p_cit = p.begin();
|
||||
|
||||
const int chain_dof = chain.DoF();
|
||||
int n;
|
||||
|
||||
for (n=0; n < chain_dof;out_seg_it ++) {
|
||||
n += out_seg_it->SetAngles(p_cit + n);
|
||||
}
|
||||
|
||||
chain.UpdateGlobalTransformations();
|
||||
chain.ComputeJacobian();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
IK_CGChainSolver::
|
||||
~IK_CGChainSolver(
|
||||
){
|
||||
//nothing to do
|
||||
};
|
||||
|
||||
|
||||
IK_CGChainSolver::
|
||||
IK_CGChainSolver(
|
||||
){
|
||||
//nothing to do;
|
||||
};
|
||||
|
||||
|
|
@ -1,211 +0,0 @@
|
|||
/**
|
||||
* $Id$
|
||||
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* as published by the Free Software Foundation; either version 2
|
||||
* of the License, or (at your option) any later version. The Blender
|
||||
* Foundation also sells licenses for use in proprietary software under
|
||||
* the Blender License. See http://www.blender.org/BL/ for information
|
||||
* about this.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software Foundation,
|
||||
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
*
|
||||
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
||||
* All rights reserved.
|
||||
*
|
||||
* The Original Code is: all of this file.
|
||||
*
|
||||
* Contributor(s): none yet.
|
||||
*
|
||||
* ***** END GPL/BL DUAL LICENSE BLOCK *****
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include "IK_Chain.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
IK_Chain::
|
||||
IK_Chain(
|
||||
)
|
||||
{
|
||||
// nothing to do;
|
||||
};
|
||||
|
||||
const
|
||||
vector<IK_Segment> &
|
||||
IK_Chain::
|
||||
Segments(
|
||||
) const {
|
||||
return m_segments;
|
||||
};
|
||||
|
||||
vector<IK_Segment> &
|
||||
IK_Chain::
|
||||
Segments(
|
||||
){
|
||||
return m_segments;
|
||||
};
|
||||
|
||||
void
|
||||
IK_Chain::
|
||||
UpdateGlobalTransformations(
|
||||
){
|
||||
|
||||
// now iterate through the segment list
|
||||
// compute their local transformations if needed
|
||||
|
||||
// assign their global transformations
|
||||
// (relative to chain origin)
|
||||
|
||||
vector<IK_Segment>::const_iterator s_end = m_segments.end();
|
||||
vector<IK_Segment>::iterator s_it = m_segments.begin();
|
||||
|
||||
MT_Transform global;
|
||||
|
||||
global.setIdentity();
|
||||
|
||||
for (; s_it != s_end; ++s_it) {
|
||||
s_it->UpdateGlobal(global);
|
||||
global = s_it->GlobalTransform();
|
||||
}
|
||||
// compute the position of the end effector and it's pose
|
||||
|
||||
const MT_Transform &last_t = m_segments.back().GlobalTransform();
|
||||
m_end_effector = last_t.getOrigin();
|
||||
|
||||
MT_Matrix3x3 last_basis = last_t.getBasis();
|
||||
last_basis.transpose();
|
||||
MT_Vector3 m_end_pose = last_basis[2];
|
||||
|
||||
|
||||
};
|
||||
|
||||
const
|
||||
TNT::Matrix<MT_Scalar> &
|
||||
IK_Chain::
|
||||
Jacobian(
|
||||
) const {
|
||||
return m_jacobian;
|
||||
} ;
|
||||
|
||||
|
||||
const
|
||||
TNT::Matrix<MT_Scalar> &
|
||||
IK_Chain::
|
||||
TransposedJacobian(
|
||||
) const {
|
||||
return m_t_jacobian;
|
||||
};
|
||||
|
||||
void
|
||||
IK_Chain::
|
||||
ComputeJacobian(
|
||||
){
|
||||
// let's assume that the chain's global transfomations
|
||||
// have already been computed.
|
||||
|
||||
int dof = DoF();
|
||||
|
||||
int num_segs = m_segments.size();
|
||||
vector<IK_Segment>::const_iterator segs = m_segments.begin();
|
||||
|
||||
m_t_jacobian.newsize(dof,3);
|
||||
m_jacobian.newsize(3,dof);
|
||||
|
||||
// compute the transposed jacobian first
|
||||
|
||||
int n;
|
||||
int i = 0;
|
||||
|
||||
|
||||
for (n= 0; n < num_segs; n++) {
|
||||
|
||||
const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis();
|
||||
const MT_Vector3 &origin = segs[n].GlobalSegmentStart();
|
||||
|
||||
MT_Vector3 p = origin-m_end_effector;
|
||||
|
||||
// iterate through the active angle vectors of this segment
|
||||
|
||||
int angle_ind =0;
|
||||
int seg_dof = segs[n].DoF();
|
||||
|
||||
const std::vector<MT_Vector3> & angle_vectors = segs[n].AngleVectors();
|
||||
|
||||
for (angle_ind = 0;angle_ind <seg_dof; angle_ind++,i++) {
|
||||
|
||||
MT_Vector3 angle_axis = angle_vectors[angle_ind];
|
||||
|
||||
MT_Vector3 a = basis * angle_axis;
|
||||
MT_Vector3 pca = p.cross(a);
|
||||
|
||||
m_t_jacobian(i + 1,1) = pca.x();
|
||||
m_t_jacobian(i + 1,2) = pca.y();
|
||||
m_t_jacobian(i + 1,3) = pca.z();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// get the origina1 jacobain
|
||||
|
||||
TNT::transpose(m_t_jacobian,m_jacobian);
|
||||
};
|
||||
|
||||
MT_Vector3
|
||||
IK_Chain::
|
||||
EndEffector(
|
||||
) const {
|
||||
return m_end_effector;
|
||||
};
|
||||
|
||||
MT_Vector3
|
||||
IK_Chain::
|
||||
EndPose(
|
||||
) const {
|
||||
return m_end_pose;
|
||||
};
|
||||
|
||||
|
||||
int
|
||||
IK_Chain::
|
||||
DoF(
|
||||
) const {
|
||||
|
||||
// iterate through the segs and compute the DOF
|
||||
|
||||
vector<IK_Segment>::const_iterator s_end = m_segments.end();
|
||||
vector<IK_Segment>::const_iterator s_it = m_segments.begin();
|
||||
|
||||
int dof = 0;
|
||||
|
||||
for (;s_it != s_end; ++s_it) {
|
||||
dof += s_it->DoF();
|
||||
}
|
||||
|
||||
return dof;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,165 +0,0 @@
|
|||
/**
|
||||
* $Id$
|
||||
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* as published by the Free Software Foundation; either version 2
|
||||
* of the License, or (at your option) any later version. The Blender
|
||||
* Foundation also sells licenses for use in proprietary software under
|
||||
* the Blender License. See http://www.blender.org/BL/ for information
|
||||
* about this.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software Foundation,
|
||||
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
*
|
||||
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
||||
* All rights reserved.
|
||||
*
|
||||
* The Original Code is: all of this file.
|
||||
*
|
||||
* Contributor(s): none yet.
|
||||
*
|
||||
* ***** END GPL/BL DUAL LICENSE BLOCK *****
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include "IK_ConjugateGradientSolver.h"
|
||||
|
||||
#define EPS 1.0e-10
|
||||
|
||||
IK_ConjugateGradientSolver *
|
||||
IK_ConjugateGradientSolver::
|
||||
New(
|
||||
){
|
||||
return new IK_ConjugateGradientSolver();
|
||||
};
|
||||
|
||||
IK_ConjugateGradientSolver::
|
||||
~IK_ConjugateGradientSolver(
|
||||
){
|
||||
//nothing to do
|
||||
};
|
||||
|
||||
// Compute the minimum of the potenial function
|
||||
// starting at point p. On return p contains the
|
||||
// computed minima, iter the number of iterations performed,
|
||||
// fret the potenial value at the minima
|
||||
|
||||
void
|
||||
IK_ConjugateGradientSolver::
|
||||
Solve(
|
||||
TNT::Vector<MT_Scalar> &p,
|
||||
MT_Scalar ftol,
|
||||
int &iter,
|
||||
MT_Scalar &fret,
|
||||
DifferentiablePotenialFunctionNd &potenial,
|
||||
int max_its
|
||||
){
|
||||
|
||||
int j;
|
||||
MT_Scalar gg,gam,fp,dgg;
|
||||
|
||||
int n = potenial.Dimension();
|
||||
ArmVectors(n);
|
||||
|
||||
// initialize --- FIXME we probably have these
|
||||
// values to hand already.
|
||||
|
||||
fp = potenial.Evaluate(p);
|
||||
potenial.Derivative(p,m_xi);
|
||||
|
||||
for (j = 1; j<=n; j++) {
|
||||
m_g(j) = -m_xi(j);
|
||||
m_xi(j) = m_h(j) = m_g(j);
|
||||
}
|
||||
for (iter =1;iter <= max_its; iter++) {
|
||||
LineMinimize(p,m_xi,fret,potenial);
|
||||
|
||||
if (2 *TNT::abs(fret-fp) <= ftol*(TNT::abs(fret) + TNT::abs(fp) + EPS)) {
|
||||
return;
|
||||
}
|
||||
fp = fret;
|
||||
potenial.Derivative(p,m_xi);
|
||||
dgg = gg = 0.0;
|
||||
|
||||
for (j = 1; j<=n;j++) {
|
||||
gg += m_g(j)*m_g(j);
|
||||
//dgg+= xi(j)*xi(j);
|
||||
dgg += (m_xi(j) + m_g(j))*m_xi(j);
|
||||
}
|
||||
if (gg == 0.0) {
|
||||
return;
|
||||
}
|
||||
gam = dgg/gg;
|
||||
|
||||
for (j = 1; j<=n; j++) {
|
||||
m_g(j) = -m_xi(j);
|
||||
m_xi(j) = m_h(j) = m_g(j) + gam*m_h(j);
|
||||
}
|
||||
}
|
||||
// FIXME throw exception
|
||||
//assert(false);
|
||||
};
|
||||
|
||||
|
||||
IK_ConjugateGradientSolver::
|
||||
IK_ConjugateGradientSolver(
|
||||
){
|
||||
//nothing to do
|
||||
}
|
||||
|
||||
void
|
||||
IK_ConjugateGradientSolver::
|
||||
ArmVectors(
|
||||
int dimension
|
||||
){
|
||||
m_g.newsize(dimension);
|
||||
m_h.newsize(dimension);
|
||||
m_xi.newsize(dimension);
|
||||
m_xi_temp.newsize(dimension);
|
||||
};
|
||||
|
||||
void
|
||||
IK_ConjugateGradientSolver::
|
||||
LineMinimize(
|
||||
TNT::Vector<MT_Scalar> & p,
|
||||
const TNT::Vector<MT_Scalar> & xi,
|
||||
MT_Scalar &fret,
|
||||
DifferentiablePotenialFunctionNd &potenial
|
||||
){
|
||||
MT_Scalar ax(0),bx(1); // initial bracket guess
|
||||
MT_Scalar cx,fa,fb,fc;
|
||||
|
||||
MT_Scalar xmin(0); // the 1d function minima
|
||||
|
||||
potenial.SetLineVector(p,xi);
|
||||
IK_LineMinimizer::InitialBracket1d(ax,bx,cx,fa,fb,fc,potenial);
|
||||
fret = IK_LineMinimizer::DerivativeBrent1d(ax,bx,cx,potenial,xmin,0.001);
|
||||
|
||||
// x_min in minimum along line and corresponds with
|
||||
// p[] + x_min *xi[]
|
||||
|
||||
TNT::vectorscale(m_xi_temp,xi,xmin);
|
||||
TNT::vectoradd(p,m_xi_temp);
|
||||
};
|
||||
|
||||
|
||||
#undef EPS
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,270 +0,0 @@
|
|||
/**
|
||||
* $Id$
|
||||
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* as published by the Free Software Foundation; either version 2
|
||||
* of the License, or (at your option) any later version. The Blender
|
||||
* Foundation also sells licenses for use in proprietary software under
|
||||
* the Blender License. See http://www.blender.org/BL/ for information
|
||||
* about this.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software Foundation,
|
||||
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
*
|
||||
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
||||
* All rights reserved.
|
||||
*
|
||||
* The Original Code is: all of this file.
|
||||
*
|
||||
* Contributor(s): none yet.
|
||||
*
|
||||
* ***** END GPL/BL DUAL LICENSE BLOCK *****
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include "IK_JacobianSolver.h"
|
||||
#include "TNT/svd.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
IK_JacobianSolver *
|
||||
IK_JacobianSolver::
|
||||
New(
|
||||
){
|
||||
return new IK_JacobianSolver();
|
||||
}
|
||||
|
||||
bool
|
||||
IK_JacobianSolver::
|
||||
Solve(
|
||||
IK_Chain &chain,
|
||||
const MT_Vector3 &g_position,
|
||||
const MT_Vector3 &g_pose,
|
||||
const MT_Scalar tolerance,
|
||||
const int max_iterations,
|
||||
const MT_Scalar max_angle_change
|
||||
){
|
||||
|
||||
ArmMatrices(chain.DoF());
|
||||
|
||||
for (int iterations = 0; iterations < max_iterations; iterations++) {
|
||||
|
||||
|
||||
MT_Vector3 end_effector = chain.EndEffector();
|
||||
|
||||
MT_Vector3 d_pos = g_position - end_effector;
|
||||
const MT_Scalar x_length = d_pos.length();
|
||||
|
||||
if (x_length < tolerance) {
|
||||
return true;
|
||||
}
|
||||
|
||||
chain.ComputeJacobian();
|
||||
ComputeInverseJacobian(chain,x_length,max_angle_change);
|
||||
|
||||
ComputeBetas(chain,d_pos);
|
||||
UpdateChain(chain);
|
||||
chain.UpdateGlobalTransformations();
|
||||
}
|
||||
|
||||
return false;
|
||||
};
|
||||
|
||||
IK_JacobianSolver::
|
||||
~IK_JacobianSolver(
|
||||
){
|
||||
// nothing to do
|
||||
}
|
||||
|
||||
|
||||
IK_JacobianSolver::
|
||||
IK_JacobianSolver(
|
||||
){
|
||||
// nothing to do
|
||||
};
|
||||
|
||||
void
|
||||
IK_JacobianSolver::
|
||||
ComputeBetas(
|
||||
IK_Chain &chain,
|
||||
const MT_Vector3 d_pos
|
||||
){
|
||||
|
||||
m_beta = 0;
|
||||
|
||||
m_beta[0] = d_pos.x();
|
||||
m_beta[1] = d_pos.y();
|
||||
m_beta[2] = d_pos.z();
|
||||
|
||||
TNT::matmult(m_d_theta,m_svd_inverse,m_beta);
|
||||
|
||||
};
|
||||
|
||||
|
||||
int
|
||||
IK_JacobianSolver::
|
||||
ComputeInverseJacobian(
|
||||
IK_Chain &chain,
|
||||
const MT_Scalar x_length,
|
||||
const MT_Scalar max_angle_change
|
||||
) {
|
||||
|
||||
int dimension = 0;
|
||||
|
||||
m_svd_u = 0;
|
||||
|
||||
// copy first 3 rows of jacobian into m_svd_u
|
||||
|
||||
int row, column;
|
||||
|
||||
for (row = 0; row < 3; row ++) {
|
||||
for (column = 0; column < chain.Jacobian().num_cols(); column ++) {
|
||||
m_svd_u[row][column] = chain.Jacobian()[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
m_svd_w = 0;
|
||||
m_svd_v = 0;
|
||||
|
||||
TNT::SVD_a(m_svd_u,m_svd_w,m_svd_v);
|
||||
|
||||
// invert the SVD and compute inverse
|
||||
|
||||
TNT::transpose(m_svd_v,m_svd_v_t);
|
||||
TNT::transpose(m_svd_u,m_svd_u_t);
|
||||
|
||||
// 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.
|
||||
|
||||
// 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 m_svd_w value
|
||||
|
||||
int i = 0;
|
||||
|
||||
MT_Scalar w_min = MT_INFINITY;
|
||||
|
||||
// anything below epsilon is treated as zero
|
||||
|
||||
MT_Scalar epsilon = 1e-10;
|
||||
|
||||
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];
|
||||
}
|
||||
}
|
||||
MT_Scalar lambda = 0;
|
||||
|
||||
MT_Scalar d = x_length/max_angle_change;
|
||||
|
||||
if (w_min <= d/2) {
|
||||
lambda = d/2;
|
||||
} else
|
||||
if (w_min < d) {
|
||||
lambda = sqrt(w_min*(d - w_min));
|
||||
} else {
|
||||
lambda = 0;
|
||||
}
|
||||
|
||||
lambda *= lambda;
|
||||
|
||||
for (i= 0; i < m_svd_w.size(); i++) {
|
||||
if (m_svd_w[i] < epsilon) {
|
||||
m_svd_w[i] = 0;
|
||||
} else {
|
||||
m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
|
||||
}
|
||||
}
|
||||
|
||||
m_svd_w_diag.diagonal(m_svd_w);
|
||||
|
||||
// FIXME optimize these matrix multiplications
|
||||
// using the fact that m_svd_w_diag is diagonal!
|
||||
|
||||
TNT::matmult(m_svd_temp1,m_svd_w_diag,m_svd_u_t);
|
||||
TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
|
||||
return dimension;
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
IK_JacobianSolver::
|
||||
UpdateChain(
|
||||
IK_Chain &chain
|
||||
){
|
||||
|
||||
// iterate through the set of angles and
|
||||
// update their values from the d_thetas
|
||||
|
||||
int n;
|
||||
vector<IK_Segment> &segs = chain.Segments();
|
||||
|
||||
int chain_dof = chain.DoF();
|
||||
int seg_ind = 0;
|
||||
for (n=0; n < chain_dof;seg_ind ++) {
|
||||
n += segs[seg_ind].IncrementAngles(m_d_theta.begin() + n);
|
||||
}
|
||||
};
|
||||
|
||||
void
|
||||
IK_JacobianSolver::
|
||||
ArmMatrices(
|
||||
int dof
|
||||
){
|
||||
|
||||
m_beta.newsize(dof);
|
||||
m_d_theta.newsize(dof);
|
||||
|
||||
m_svd_u.newsize(dof,dof);
|
||||
m_svd_v.newsize(dof,dof);
|
||||
m_svd_w.newsize(dof);
|
||||
|
||||
m_svd_u = 0;
|
||||
m_svd_v = 0;
|
||||
m_svd_w = 0;
|
||||
|
||||
m_svd_u_t.newsize(dof,dof);
|
||||
m_svd_v_t.newsize(dof,dof);
|
||||
m_svd_w_diag.newsize(dof,dof);
|
||||
m_svd_inverse.newsize(dof,dof);
|
||||
m_svd_temp1.newsize(dof,dof);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,281 +0,0 @@
|
|||
/**
|
||||
* $Id$
|
||||
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* as published by the Free Software Foundation; either version 2
|
||||
* of the License, or (at your option) any later version. The Blender
|
||||
* Foundation also sells licenses for use in proprietary software under
|
||||
* the Blender License. See http://www.blender.org/BL/ for information
|
||||
* about this.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software Foundation,
|
||||
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
*
|
||||
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
||||
* All rights reserved.
|
||||
*
|
||||
* The Original Code is: all of this file.
|
||||
*
|
||||
* Contributor(s): none yet.
|
||||
*
|
||||
* ***** END GPL/BL DUAL LICENSE BLOCK *****
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include "IK_Segment.h"
|
||||
|
||||
|
||||
IK_Segment::
|
||||
IK_Segment (
|
||||
const MT_Point3 tr1,
|
||||
const MT_Matrix3x3 A,
|
||||
const MT_Scalar length,
|
||||
const bool pitch_on,
|
||||
const bool yaw_on,
|
||||
const bool role_on
|
||||
){
|
||||
m_transform.setOrigin(tr1);
|
||||
m_transform.setBasis(A);
|
||||
m_angles[0] =MT_Scalar(0);
|
||||
m_angles[1] =MT_Scalar(0);
|
||||
m_angles[2] =MT_Scalar(0);
|
||||
|
||||
m_active_angles[0] = role_on;
|
||||
m_active_angles[1] = yaw_on;
|
||||
m_active_angles[2] = pitch_on;
|
||||
m_length = length;
|
||||
|
||||
if (role_on) {
|
||||
m_angle_vectors.push_back(MT_Vector3(1,0,0));
|
||||
}
|
||||
if (yaw_on) {
|
||||
m_angle_vectors.push_back(MT_Vector3(0,1,0));
|
||||
}
|
||||
if (pitch_on) {
|
||||
m_angle_vectors.push_back(MT_Vector3(0,0,1));
|
||||
}
|
||||
UpdateLocalTransform();
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
IK_Segment::
|
||||
IK_Segment (
|
||||
) {
|
||||
m_transform.setIdentity();
|
||||
|
||||
m_angles[0] =MT_Scalar(0);
|
||||
m_angles[1] =MT_Scalar(0);
|
||||
m_angles[2] =MT_Scalar(0);
|
||||
|
||||
m_active_angles[0] = false;
|
||||
m_active_angles[1] = false;
|
||||
m_active_angles[2] = false;
|
||||
m_length = MT_Scalar(1);
|
||||
|
||||
UpdateLocalTransform();
|
||||
}
|
||||
|
||||
|
||||
|
||||
// accessors
|
||||
////////////
|
||||
|
||||
// The length of the segment
|
||||
|
||||
const
|
||||
MT_Scalar
|
||||
IK_Segment::
|
||||
Length(
|
||||
) const {
|
||||
return m_length;
|
||||
}
|
||||
|
||||
|
||||
// This is the transform from adjacent
|
||||
// coordinate systems in the chain.
|
||||
|
||||
const
|
||||
MT_Transform &
|
||||
IK_Segment::
|
||||
LocalTransform(
|
||||
) const {
|
||||
return m_local_transform;
|
||||
}
|
||||
|
||||
void
|
||||
IK_Segment::
|
||||
UpdateGlobal(
|
||||
const MT_Transform & global
|
||||
){
|
||||
|
||||
// compute the global transform
|
||||
// and the start of the segment in global coordinates.
|
||||
|
||||
m_seg_start = global * m_transform.getOrigin();
|
||||
m_global_transform = global * m_local_transform;
|
||||
}
|
||||
|
||||
const
|
||||
MT_Transform &
|
||||
IK_Segment::
|
||||
GlobalTransform(
|
||||
) const {
|
||||
return m_global_transform;
|
||||
}
|
||||
|
||||
const
|
||||
MT_Vector3 &
|
||||
IK_Segment::
|
||||
GlobalSegmentStart(
|
||||
) const{
|
||||
return m_seg_start;
|
||||
}
|
||||
|
||||
|
||||
// Return the number of Degrees of Freedom
|
||||
// for this segment
|
||||
|
||||
int
|
||||
IK_Segment::
|
||||
DoF(
|
||||
) const {
|
||||
return
|
||||
(m_active_angles[0] == true) +
|
||||
(m_active_angles[1] == true) +
|
||||
(m_active_angles[2] == true);
|
||||
}
|
||||
|
||||
|
||||
// suspect interface...
|
||||
// Increment the active angles (at most 3) by
|
||||
// d_theta. Which angles are incremented depends
|
||||
// on which are active. It returns DoF
|
||||
|
||||
int
|
||||
IK_Segment::
|
||||
IncrementAngles(
|
||||
MT_Scalar *d_theta
|
||||
){
|
||||
int i =0;
|
||||
if (m_active_angles[0]) {
|
||||
m_angles[0] += d_theta[i];
|
||||
i++;
|
||||
}
|
||||
if (m_active_angles[1]) {
|
||||
m_angles[1] += d_theta[i];
|
||||
i++;
|
||||
}
|
||||
if (m_active_angles[2]) {
|
||||
m_angles[2] += d_theta[i];
|
||||
i++;
|
||||
}
|
||||
UpdateLocalTransform();
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
IK_Segment::
|
||||
SetAngles(
|
||||
const MT_Scalar *angles
|
||||
){
|
||||
int i =0;
|
||||
if (m_active_angles[0]) {
|
||||
m_angles[0] = angles[i];
|
||||
i++;
|
||||
}
|
||||
if (m_active_angles[1]) {
|
||||
m_angles[1] = angles[i];
|
||||
i++;
|
||||
}
|
||||
if (m_active_angles[2]) {
|
||||
m_angles[2] = angles[i];
|
||||
i++;
|
||||
}
|
||||
UpdateLocalTransform();
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
IK_Segment::
|
||||
UpdateLocalTransform(
|
||||
){
|
||||
// The local transformation is defined by
|
||||
// a user defined translation and rotation followed by
|
||||
// rotation by (roll,pitch,yaw) followed by
|
||||
// a translation in x of m_length
|
||||
|
||||
MT_Quaternion rotx,roty,rotz;
|
||||
|
||||
rotx.setRotation(MT_Vector3(1,0,0),m_angles[0]);
|
||||
roty.setRotation(MT_Vector3(0,1,0),m_angles[1]);
|
||||
rotz.setRotation(MT_Vector3(0,0,1),m_angles[2]);
|
||||
|
||||
MT_Quaternion rot = rotx * roty *rotz;
|
||||
|
||||
MT_Transform rx(MT_Point3(0,0,0),rot);
|
||||
|
||||
MT_Transform translation;
|
||||
translation.setIdentity();
|
||||
translation.translate(MT_Vector3(0,m_length,0));
|
||||
|
||||
m_local_transform = m_transform * rx * translation;
|
||||
};
|
||||
|
||||
|
||||
const
|
||||
std::vector<MT_Vector3> &
|
||||
IK_Segment::
|
||||
AngleVectors(
|
||||
) const{
|
||||
return m_angle_vectors;
|
||||
};
|
||||
|
||||
MT_Scalar
|
||||
IK_Segment::
|
||||
ActiveAngle(
|
||||
int i
|
||||
) const {
|
||||
MT_assert((i >=0) && (i < DoF()));
|
||||
|
||||
// umm want to return the ith active angle
|
||||
// and not the ith angle
|
||||
|
||||
int j;
|
||||
int angles = -1;
|
||||
for (j=0;j < 3;j++) {
|
||||
if (m_active_angles[j]) angles ++;
|
||||
if (i == angles) return m_angles[j];
|
||||
}
|
||||
return m_angles[0];
|
||||
}
|
||||
|
||||
MT_Scalar
|
||||
IK_Segment::
|
||||
Angle(
|
||||
int i
|
||||
) const {
|
||||
MT_assert((i >=0) && (i < 3));
|
||||
return m_angles[i];
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue