Revision: 20749
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20749
Author: ben2610
Date: 2009-06-09 13:27:30 +0200 (Tue, 09 Jun 2009)
Log Message:
-----------
iTaSC: implementation of spherical joint. Convergence and stability of the
solver is much improved. iTaSC behaves almost fully like IK_solver, but
normally faster and more consistently. Some performance tests should be done.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Armature.hpp
branches/ge_dev/intern/itasc/Scene.cpp
branches/ge_dev/intern/itasc/WDLSSolver.cpp
branches/ge_dev/intern/itasc/WSDLSSolver.cpp
branches/ge_dev/intern/itasc/kdl/chain.cpp
branches/ge_dev/intern/itasc/kdl/chainfksolverpos_recursive.cpp
branches/ge_dev/intern/itasc/kdl/chainjnttojacsolver.cpp
branches/ge_dev/intern/itasc/kdl/frames.hpp
branches/ge_dev/intern/itasc/kdl/frames.inl
branches/ge_dev/intern/itasc/kdl/joint.cpp
branches/ge_dev/intern/itasc/kdl/joint.hpp
branches/ge_dev/intern/itasc/kdl/segment.cpp
branches/ge_dev/intern/itasc/kdl/segment.hpp
branches/ge_dev/intern/itasc/kdl/tree.cpp
branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp
branches/ge_dev/intern/itasc/kdl/treejnttojacsolver.cpp
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp 2009-06-09 11:26:45 UTC (rev
20748)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-06-09 11:27:30 UTC (rev
20749)
@@ -144,7 +144,7 @@
return true;
}
-bool Armature::addSegment(const std::string& segment_name, const std::string&
hook_name, const Joint& joint, double q_rest, const Frame& f_tip, const
Inertia& M)
+bool Armature::addSegment(const std::string& segment_name, const std::string&
hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const
Inertia& M)
{
if (m_finalized)
return false;
@@ -152,24 +152,27 @@
Segment segment(joint, f_tip, M);
if (!m_tree.addSegment(segment, segment_name, hook_name))
return false;
-
- if (joint.getType() != Joint::None) {
- m_joints.push_back(q_rest);
- m_njoint++;
+ int ndof = joint.getNDof();
+ for (int dof=0; dof<ndof; dof++) {
+ Joint_struct js((&q_rest)[dof], joint.getType(), ndof);
+ m_joints.push_back(js);
}
+ m_njoint+=ndof;
return true;
}
-bool Armature::getSegment(const std::string& name, const Joint* &p_joint,
double &q_rest, double &q, const Frame* &p_tip)
+bool Armature::getSegment(const std::string& name, const unsigned int q_size,
const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
{
SegmentMap::const_iterator sit = m_tree.getSegment(name);
if (sit == m_tree.getSegments().end())
return false;
p_joint = &sit->second.segment.getJoint();
+ if (q_size < p_joint->getNDof())
+ return false;
p_tip = &sit->second.segment.getFrameToTip();
- if (p_joint->getType() != Joint::None) {
- q_rest = m_joints[sit->second.q_nr];
- q = m_qKdl(sit->second.q_nr);
+ for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
+ (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
+ (&q)[dof] = m_qKdl(sit->second.q_nr+dof);
}
return true;
}
@@ -180,6 +183,7 @@
return 0.0;
double maxJoint = 0.0;
for (unsigned int i=0; i<m_njoint; i++) {
+ // this is a very rough calculation, it doesn't work well for
spherical joint
double joint = fabs(m_oldqKdl(i)-m_qKdl(i));
if (maxJoint < joint)
maxJoint = joint;
@@ -190,7 +194,8 @@
int Armature::addConstraint(const std::string& segment_name,
ConstraintCallback _function, void* _param, bool _freeParam, bool _substep)
{
SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
- if (segment_it == m_tree.getSegments().end() ||
segment_it->second.segment.getJoint().getType() == Joint::None) {
+ // not suitable for NDof joints
+ if (segment_it == m_tree.getSegments().end() ||
segment_it->second.segment.getJoint().getNDof() != 1) {
if (_freeParam && _param)
free(_param);
return -1;
@@ -222,7 +227,7 @@
m_constraints.push_back(pConstraint);
// desired value = rest position
//(suitable for joint limit constraint, maybe changed by user in
callback)
- pConstraint->value.yd = m_joints[segment_it->second.q_nr];
+ pConstraint->value.yd = m_joints[segment_it->second.q_nr].rest;
return m_nconstraint++;
}
@@ -260,7 +265,8 @@
int Armature::addLimitConstraint(const std::string& segment_name, double _min,
double _max, double _threshold, double _maxWeight, double _slope)
{
SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
- if (segment_it == m_tree.getSegments().end() ||
segment_it->second.segment.getJoint().getType() == Joint::None) {
+ if (segment_it == m_tree.getSegments().end() ||
segment_it->second.segment.getJoint().getNDof() != 1) {
+ // not suitable for NDof joints
return -1;
}
if (segment_it->second.segment.getJoint().getType() < Joint::TransX) {
@@ -307,6 +313,7 @@
initialize(m_njoint, m_nconstraint, m_neffector);
for (i=0; i<m_nconstraint; i++) {
JointConstraint_struct* pConstraint = m_constraints[i];
+ // constraints for 1DOF only
m_Cq(i,pConstraint->segment->second.q_nr) = 1.0;
m_Wy(i) =
pConstraint->values.alpha/(pConstraint->values.tolerance*pConstraint->values.feedback);
}
@@ -317,7 +324,7 @@
m_oldqKdl.resize(m_njoint);
m_qdotKdl.resize(m_njoint);
for (i=0; i<m_njoint; i++) {
- m_oldqKdl(i) = m_qKdl(i) = m_joints[i];
+ m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
}
updateJacobian();
m_finalized = true;
@@ -330,10 +337,28 @@
if (!m_finalized)
return;
- for(unsigned int i=0;i<m_nq;i++)
- m_qdotKdl(i)=m_qdot(i)*timestamp.realTimestep;
- Add(m_qKdl,m_qdotKdl,m_qKdl);
+ // integration, for spherical joint we must use a more sophisticated
method
+ unsigned int q_nr;
+ for (q_nr=0; q_nr<m_nq; ++q_nr)
+ m_qdotKdl(q_nr)=m_qdot(q_nr);
+ for (unsigned int q_nr=0; q_nr<m_nq; ) {
+ Joint_struct& joint = m_joints[q_nr];
+ switch (joint.type) {
+ case KDL::Joint::Sphere:
+ {
+ double* qdot=&m_qdotKdl(q_nr);
+ double* q=&m_qKdl(q_nr);
+
(KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)*KDL::Rot(KDL::Vector(q))).GetRot().GetValue(q);
+ break;
+ }
+ default:
+ for (unsigned int i=0; i<joint.ndof; i++)
+ m_qKdl(q_nr+i) +=
m_qdotKdl(q_nr+i)*timestamp.realTimestep;
+ }
+ q_nr += joint.ndof;
+ }
+
if (!timestamp.substep && timestamp.cache) {
pushQ(timestamp.cacheTimestamp);
pushConstraints(timestamp.cacheTimestamp);
@@ -375,13 +400,6 @@
return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) <
0) ? false : true;
}
-double Armature::getJoint(unsigned int joint)
-{
- if (m_finalized && joint < m_njoint)
- return m_qKdl(joint);
- return 0.0;
-}
-
void Armature::updateControlOutput(const Timestamp& timestamp)
{
if (!m_finalized)
Modified: branches/ge_dev/intern/itasc/Armature.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.hpp 2009-06-09 11:26:45 UTC (rev
20748)
+++ branches/ge_dev/intern/itasc/Armature.hpp 2009-06-09 11:27:30 UTC (rev
20749)
@@ -21,14 +21,13 @@
Armature();
virtual ~Armature();
- bool addSegment(const std::string& segment_name, const std::string&
hook_name, const Joint& joint, double q_rest=0.0, const Frame&
f_tip=F_identity, const Inertia& M = Inertia::Zero());
+ bool addSegment(const std::string& segment_name, const std::string&
hook_name, const Joint& joint, const double& q_rest, const Frame&
f_tip=F_identity, const Inertia& M = Inertia::Zero());
// general purpose constraint on joint
int addConstraint(const std::string& segment_name, ConstraintCallback
_function, void* _param=NULL, bool _freeParam=false, bool _substep=false);
// specific limit constraint on joint
int addLimitConstraint(const std::string& segment_name, double _min,
double _max, double _threshold, double _maxWeight=1000.0, double _slope=1.0);
- double getJoint(unsigned int joint);
double getMaxJointChange();
- bool getSegment(const std::string& segment_name, const Joint* &p_joint,
double &q_rest, double &q, const Frame* &p_tip);
+ bool getSegment(const std::string& segment_name, const unsigned int
q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
bool getRelativeFrame(Frame& result, const std::string& segment_name,
const std::string& base_name=m_root);
virtual void finalize();
@@ -80,7 +79,15 @@
};
typedef std::vector<JointConstraint_struct*> JointConstraintList;
- typedef std::vector<double> JointList;
+ struct Joint_struct {
+ double rest;
+ KDL::Joint::JointType type;
+ unsigned int ndof;
+
+ Joint_struct(double _rest, KDL::Joint::JointType _type,
unsigned int _ndof) :
+ rest(_rest), type(_type), ndof(_ndof) {}
+ };
+ typedef std::vector<Joint_struct> JointList;
protected:
virtual void updateJacobian();
@@ -102,6 +109,7 @@
JntArray m_oldqKdl;
JntArray m_qdotKdl;
Jacobian* m_jac;
+
KDL::TreeJntToJacSolver* m_jacsolver;
KDL::TreeFkSolverPos_recursive* m_fksolver;
EffectorList m_effectors;
Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp 2009-06-09 11:26:45 UTC (rev
20748)
+++ branches/ge_dev/intern/itasc/Scene.cpp 2009-06-09 11:27:30 UTC (rev
20749)
@@ -383,7 +383,7 @@
// and joint limit gain variation
// We will pass the joint velocity to each object and
they will recommend a maximum timestep
timesubstep = timeleft;
- double maxsubstep = timestep/nlcoef;
+ double maxsubstep = nlcoef*0.06;
if (maxsubstep < m_minstep)
maxsubstep = m_minstep;
if (timesubstep > maxsubstep)
@@ -398,7 +398,7 @@
cs->task->getMaxTimestep(timesubstep);
}
// use substep that are dividers of timestep for more
regularity
- timesubstep =
timestep/(1.0+floor((timestep/timesubstep)-0.5));
+ timesubstep =
timestep/(1.0+floor((timestep/timesubstep)-0.333));
if (timesubstep >= timeleft-(m_minstep/2.0)) {
timesubstep = timeleft;
numsubstep = 1;
Modified: branches/ge_dev/intern/itasc/WDLSSolver.cpp
===================================================================
--- branches/ge_dev/intern/itasc/WDLSSolver.cpp 2009-06-09 11:26:45 UTC (rev
20748)
+++ branches/ge_dev/intern/itasc/WDLSSolver.cpp 2009-06-09 11:27:30 UTC (rev
20749)
@@ -69,9 +69,9 @@
//qdot=Wq*V*S^-1*U'*Wy'*ydot
qdot=(m_WqV*m_SinvWyUt_ydot).lazy();
if (maxDeltaS == e_scalar(0.0))
- nlcoef = e_scalar(1.0/KDL::epsilon);
+ nlcoef = e_scalar(KDL::epsilon);
else
- nlcoef = maxS/(maxS-maxDeltaS)/e_scalar(2.0);
+ nlcoef = (maxS-maxDeltaS)/maxS;
return true;
}
Modified: branches/ge_dev/intern/itasc/WSDLSSolver.cpp
===================================================================
--- branches/ge_dev/intern/itasc/WSDLSSolver.cpp 2009-06-09 11:26:45 UTC
(rev 20748)
+++ branches/ge_dev/intern/itasc/WSDLSSolver.cpp 2009-06-09 11:27:30 UTC
(rev 20749)
@@ -113,9 +113,9 @@
prevS = S;
}
if (maxDeltaS == e_scalar(0.0))
- nlcoef = e_scalar(1.0/KDL::epsilon);
+ nlcoef = e_scalar(KDL::epsilon);
else
- nlcoef = maxS/(maxS-maxDeltaS)/e_scalar(2.0);
+ nlcoef = (maxS-maxDeltaS)/maxS;
return true;
}
Modified: branches/ge_dev/intern/itasc/kdl/chain.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/chain.cpp 2009-06-09 11:26:45 UTC (rev
20748)
+++ branches/ge_dev/intern/itasc/kdl/chain.cpp 2009-06-09 11:27:30 UTC (rev
20749)
@@ -53,8 +53,7 @@
{
segments.push_back(segment);
nrOfSegments++;
- if(segment.getJoint().getType()!=Joint::None)
@@ Diff output truncated at 10240 characters. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
http://lists.blender.org/mailman/listinfo/bf-blender-cvs
