[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22520] branches/itasc: iTaSC: Distance constraint, normalization of error based on armature size, choice of DLS and SDLS solver, numerical filtering for DLS solver.
Benoit Bolsee
benoit.bolsee at online.be
Sun Aug 16 17:08:10 CEST 2009
Revision: 22520
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22520
Author: ben2610
Date: 2009-08-16 17:08:09 +0200 (Sun, 16 Aug 2009)
Log Message:
-----------
iTaSC: Distance constraint, normalization of error based on armature size, choice of DLS and SDLS solver, numerical filtering for DLS solver.
The Damped Least Square solver was not available on the UI.
It is now available with two additional parameters:
Damp max and Damp eps, respectively the maximum damping
value and the singular value zone below which damping
starts to be applied. Both values are relative to armature
size, so they should remain relatively constant for various
armature configuration.
The DLS solver is useful when working with the Distance
constraint because the automatic damping method of the SDLS
solver does not work well with the distance constraint.
If you experience instabibility in singular pose, increasing
Damp max and Damp eps will improve stability at the expense
of less accuracy in the non-singular pose. Some tuning is
necessary for best compromise.
Distance constraint is available as a variant of the IK
constraint. Three mode are supported: inside, outside, on
surface. Inside and Outside variants are obtained by setting
the weight automatically to 0 when the end effector is in
the acceptable zone.
Fix bug with the DSL solver where residual joint velocity
was possible in case of totally or partially degenerated
Jacobian.
Generalization of the solver parameter interface to allow
future parameter to be added easily in the itasc plugin.
Use armature size information to clamp error on constraint.
This ensures that the robot will not become artifically
unstable when the constraint error gets arbitrary large.
Position and distance error is clamped to maximum half of
the average bone length. The amount of feedback can still
be controled via the feedback parameter.
Decoupling of feedback coefficient from constraint gain:
this makes the feedback coefficient a useable parameter.
Smaller values will improve stability but decrease accuracy.
Allow every constraint to update weight on each substep.
Previously this was only possible on the joint limit constraint.
Modified Paths:
--------------
branches/itasc/intern/itasc/Armature.cpp
branches/itasc/intern/itasc/Armature.hpp
branches/itasc/intern/itasc/ConstraintSet.cpp
branches/itasc/intern/itasc/ConstraintSet.hpp
branches/itasc/intern/itasc/CopyPose.cpp
branches/itasc/intern/itasc/CopyPose.hpp
branches/itasc/intern/itasc/Distance.cpp
branches/itasc/intern/itasc/Distance.hpp
branches/itasc/intern/itasc/Scene.cpp
branches/itasc/intern/itasc/Solver.hpp
branches/itasc/intern/itasc/WDLSSolver.cpp
branches/itasc/intern/itasc/WDLSSolver.hpp
branches/itasc/intern/itasc/WSDLSSolver.cpp
branches/itasc/intern/itasc/WSDLSSolver.hpp
branches/itasc/release/ui/buttons_data_bone.py
branches/itasc/source/blender/blenkernel/intern/action.c
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
branches/itasc/source/blender/makesdna/DNA_action_types.h
branches/itasc/source/blender/makesrna/intern/rna_pose.c
Modified: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp 2009-08-16 15:06:25 UTC (rev 22519)
+++ branches/itasc/intern/itasc/Armature.cpp 2009-08-16 15:08:09 UTC (rev 22520)
@@ -34,7 +34,8 @@
m_qdotKdl(),
m_jac(NULL),
m_jacsolver(NULL),
- m_fksolver(NULL)
+ m_fksolver(NULL),
+ m_armlength(0.0)
{
}
@@ -354,7 +355,7 @@
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);
+ m_Wy(i) = pConstraint->values.alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
}
m_jacsolver= new KDL::TreeJntToJacSolver(m_tree);
m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
@@ -366,6 +367,22 @@
m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
}
updateJacobian();
+ // estimate the maximum size of the robot arms
+ double length;
+ m_armlength = 0.0;
+ for (i=0; i<m_neffector; i++) {
+ length = 0.0;
+ KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
+ while (sit->first != "root") {
+ Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
+ length += tip.p.Norm();
+ sit = sit->second.parent;
+ }
+ if (length > m_armlength)
+ m_armlength = length;
+ }
+ if (m_armlength < KDL::epsilon)
+ m_armlength = KDL::epsilon;
m_finalized = true;
}
@@ -480,7 +497,7 @@
(*pConstraint->function)(timestamp, &pConstraint->values, 1, pConstraint->param);
}
// recompute the weight in any case, that's the most likely modification
- m_Wy(iConstraint) = pConstraint->values.alpha/(pConstraint->values.tolerance*pConstraint->values.feedback);
+ m_Wy(iConstraint) = pConstraint->values.alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
m_ydot(iConstraint)=pConstraint->value.yddot+pConstraint->values.feedback*(pConstraint->value.yd-pConstraint->value.y);
}
}
@@ -519,7 +536,7 @@
break;
}
if (m_finalized)
- m_Wy(constraintId) = pConstraint->values.alpha/(pConstraint->values.tolerance*pConstraint->values.feedback);
+ m_Wy(constraintId) = pConstraint->values.alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
}
}
return true;
Modified: branches/itasc/intern/itasc/Armature.hpp
===================================================================
--- branches/itasc/intern/itasc/Armature.hpp 2009-08-16 15:06:25 UTC (rev 22519)
+++ branches/itasc/intern/itasc/Armature.hpp 2009-08-16 15:08:09 UTC (rev 22520)
@@ -41,6 +41,10 @@
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
virtual void initCache(Cache *_cache);
virtual double getMaxTimestep(double& timestep);
+ virtual double getArmLength()
+ {
+ return m_armlength;
+ }
struct Effector_struct {
std::string name;
@@ -112,6 +116,7 @@
JntArray m_oldqKdl;
JntArray m_qdotKdl;
Jacobian* m_jac;
+ double m_armlength;
KDL::TreeJntToJacSolver* m_jacsolver;
KDL::TreeFkSolverPos_recursive* m_fksolver;
Modified: branches/itasc/intern/itasc/ConstraintSet.cpp
===================================================================
--- branches/itasc/intern/itasc/ConstraintSet.cpp 2009-08-16 15:06:25 UTC (rev 22519)
+++ branches/itasc/intern/itasc/ConstraintSet.cpp 2009-08-16 15:08:09 UTC (rev 22520)
@@ -22,7 +22,7 @@
m_internalPose(F_identity), m_externalPose(F_identity),
m_constraintCallback(NULL), m_constraintParam(NULL),
m_poseCallback(NULL), m_poseParam(NULL),
- m_toggle(false),
+ m_toggle(false),m_substep(false),
m_threshold(accuracy),m_maxIter(maximum_iterations)
{
m_maxDeltaChi = e_scalar(0.52);
@@ -33,7 +33,7 @@
m_internalPose(F_identity), m_externalPose(F_identity),
m_constraintCallback(NULL), m_constraintParam(NULL),
m_poseCallback(NULL), m_poseParam(NULL),
- m_toggle(false),
+ m_toggle(false),m_substep(false),
m_threshold(0.0),m_maxIter(0)
{
m_maxDeltaChi = e_scalar(0.52);
Modified: branches/itasc/intern/itasc/ConstraintSet.hpp
===================================================================
--- branches/itasc/intern/itasc/ConstraintSet.hpp 2009-08-16 15:06:25 UTC (rev 22519)
+++ branches/itasc/intern/itasc/ConstraintSet.hpp 2009-08-16 15:08:09 UTC (rev 22520)
@@ -63,6 +63,7 @@
ExternalPoseCallback m_poseCallback;
void* m_poseParam;
bool m_toggle;
+ bool m_substep;
double m_threshold;
unsigned int m_maxIter;
@@ -116,6 +117,8 @@
virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
virtual unsigned int getNrOfConstraints(){return m_nc;};
+ void substep(bool _substep) {m_substep=_substep;}
+ bool substep() {return m_substep;}
};
}
Modified: branches/itasc/intern/itasc/CopyPose.cpp
===================================================================
--- branches/itasc/intern/itasc/CopyPose.cpp 2009-08-16 15:06:25 UTC (rev 22519)
+++ branches/itasc/intern/itasc/CopyPose.cpp 2009-08-16 15:08:09 UTC (rev 22520)
@@ -15,11 +15,12 @@
{
const unsigned int maxPoseCacheSize = (2*(3+3*2));
-CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double accuracy, unsigned int maximum_iterations):
+CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
ConstraintSet(),
m_cache(NULL),
m_poseCCh(-1),m_poseCTs(0)
{
+ m_maxerror = armlength/2.0;
m_outputControl = (control_output & CTL_ALL);
int _nc = nBitsOn(m_outputControl);
if (!_nc)
@@ -45,21 +46,21 @@
m_values[m_nvalues].tolerance = m_pos.tolerance;
m_values[m_nvalues].id = ID_POSITION;
if (m_outputControl & CTL_POSITIONX) {
- m_Wy(_nc) = m_pos.alpha/(m_pos.tolerance*m_pos.K);
+ m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,0)=1.0;
m_posData[npos++].id = ID_POSITIONX;
if (m_outputDynamic & CTL_POSITIONX)
nposCache++;
}
if (m_outputControl & CTL_POSITIONY) {
- m_Wy(_nc) = m_pos.alpha/(m_pos.tolerance*m_pos.K);
+ m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,1)=1.0;
m_posData[npos++].id = ID_POSITIONY;
if (m_outputDynamic & CTL_POSITIONY)
nposCache++;
}
if (m_outputControl & CTL_POSITIONZ) {
- m_Wy(_nc) = m_pos.alpha/(m_pos.tolerance*m_pos.K);
+ m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,2)=1.0;
m_posData[npos++].id = ID_POSITIONZ;
if (m_outputDynamic & CTL_POSITIONZ)
@@ -79,21 +80,21 @@
m_values[m_nvalues].tolerance = m_rot.tolerance;
m_values[m_nvalues].id = ID_ROTATION;
if (m_outputControl & CTL_ROTATIONX) {
- m_Wy(_nc) = m_rot.alpha/(m_rot.tolerance*m_rot.K);
+ m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,3)=1.0;
m_rotData[nrot++].id = ID_ROTATIONX;
if (m_outputDynamic & CTL_ROTATIONX)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONY) {
- m_Wy(_nc) = m_rot.alpha/(m_rot.tolerance*m_rot.K);
+ m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,4)=1.0;
m_rotData[nrot++].id = ID_ROTATIONY;
if (m_outputDynamic & CTL_ROTATIONY)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONZ) {
- m_Wy(_nc) = m_rot.alpha/(m_rot.tolerance*m_rot.K);
+ m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,5)=1.0;
m_rotData[nrot++].id = ID_ROTATIONZ;
if (m_outputDynamic & CTL_ROTATIONZ)
@@ -189,7 +190,7 @@
for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) {
if (m_outputControl & mask) {
- m_Wy(i) = _state->alpha/(_state->tolerance*_state->K);
+ m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
if (m_outputDynamic & mask) {
_data->yd = _yval->yd = *item++;
_data->yddot = _yval->yddot = *item++;
@@ -313,7 +314,7 @@
for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
if (m_outputControl & mask) {
if (action)
- m_Wy(i) = _state->alpha/(_state->tolerance*_state->K);
+ m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
// check if this controlled output is provided
for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
if (_data->id == id) {
@@ -404,9 +405,23 @@
{
ControlState::ControlValue* _yval;
int i, j;
- for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++, mask<<=1) {
- if (m_outputControl & mask) {
- m_ydot(i)=_yval->yddot+_state->K*(_yval->yd+vel(j));
+ double coef=1.0;
+ if (mask & CTL_POSITION) {
+ // put a limit on position error
+ double len=0.0;
+ for (j=0, _yval=_state->output; j<3; j++) {
+ if (m_outputControl & (mask<<j)) {
+ len += KDL::sqr(_yval->yd+vel(j));
+ _yval++;
+ }
+ }
+ len = KDL::sqrt(len);
+ if (len > m_maxerror)
+ coef = m_maxerror/len;
+ }
+ for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
+ if (m_outputControl & (mask<<j)) {
+ m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd+vel(j));
_yval++;
i++;
}
@@ -424,19 +439,19 @@
// pose updated, recalculate the rotation
y = diff(m_internalPose,F_identity);
}
- if (m_constraintCallback) {
- // initialize first callback the application to get the current values
- int i=0;
- if (m_outputControl & CTL_POSITION) {
- updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
- }
- if (m_outputControl & CTL_ROTATION) {
- updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
- }
- if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
- setControlParameters(m_values, m_nvalues, (found)?timestamp.realTimestep:0.0);
- }
+ }
+ if (m_constraintCallback && (!timestamp.substep || m_substep)) {
+ // initialize first callback the application to get the current values
+ int i=0;
+ if (m_outputControl & CTL_POSITION) {
+ updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
+ if (m_outputControl & CTL_ROTATION) {
+ updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
+ }
+ if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
+ setControlParameters(m_values, m_nvalues, (found)?timestamp.realTimestep:0.0);
+ }
}
if (m_outputControl & CTL_POSITION) {
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list