[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22628] branches/itasc: iTaSC: NDoF joint limit supported as in Legacy solver.
Benoit Bolsee
benoit.bolsee at online.be
Wed Aug 19 12:19:40 CEST 2009
Revision: 22628
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22628
Author: ben2610
Date: 2009-08-19 12:19:40 +0200 (Wed, 19 Aug 2009)
Log Message:
-----------
iTaSC: NDoF joint limit supported as in Legacy solver.
The following 'tricks' have been used to get proper limit
enforced without spending extra CPU:
- use a 'swing'+RY decomposition for spherical joint.
The swing joint uses a rotation vector in the XZ plane.
The X/Z joint limit defined in the UI is expressed in
this coordinate system.
- Add a constraint on the X/Z value of the swing rotation.
This will create artifically 'sticky' points where the
joint will tend to go when the constraint is enforced.
- Compute the desired value for the constraint to be the
closest point on the joint limit when the joint is
outside the limit.
- Relax the constraint every other substep to let the
armature move freely towards the target. This may
cause the limit to be violated.
- Enfore the constraint on the next substep with a high
weight so that the desired joint velocity is realized.
- Set the feedback coefficient to be exactly the inverse
of the substep: this means that the desired velocity
will be such that the gap to the desired position on
the joint limit will be covered in the same substep.
Since the armature is displayed at the end of all substeps
and the number of substeps is even, only the configurations
where the joints are at the limit will be displayed. The
result is correct joint limit enforcement with no 'sticky'
point on the joint limit.
Modified Paths:
--------------
branches/itasc/intern/itasc/Armature.cpp
branches/itasc/intern/itasc/Armature.hpp
branches/itasc/intern/itasc/Cache.hpp
branches/itasc/intern/itasc/Scene.cpp
branches/itasc/intern/itasc/eigen_types.hpp
branches/itasc/intern/itasc/kdl/frames.hpp
branches/itasc/intern/itasc/kdl/frames.inl
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp 2009-08-19 09:52:13 UTC (rev 22627)
+++ branches/itasc/intern/itasc/Armature.cpp 2009-08-19 10:19:40 UTC (rev 22628)
@@ -21,6 +21,7 @@
m_tree(),
m_njoint(0),
m_nconstraint(0),
+ m_noutput(0),
m_neffector(0),
m_finalized(false),
m_cache(NULL),
@@ -56,6 +57,74 @@
m_constraints.clear();
}
+Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep):
+ segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep)
+{
+ memset(values, 0, sizeof(values));
+ memset(value, 0, sizeof(value));
+ values[0].feedback = 20.0;
+ values[1].feedback = 20.0;
+ values[2].feedback = 20.0;
+ values[0].tolerance = 1.0;
+ values[1].tolerance = 1.0;
+ values[2].tolerance = 1.0;
+ values[0].values = &value[0];
+ values[1].values = &value[1];
+ values[2].values = &value[2];
+ values[0].number = 1;
+ values[1].number = 1;
+ values[2].number = 1;
+ switch (segment->second.segment.getJoint().getType()) {
+ case Joint::RotX:
+ value[0].id = ID_JOINT_RX;
+ values[0].id = ID_JOINT_RX;
+ v_nr = 1;
+ break;
+ case Joint::RotY:
+ value[0].id = ID_JOINT_RY;
+ values[0].id = ID_JOINT_RY;
+ v_nr = 1;
+ break;
+ case Joint::RotZ:
+ value[0].id = ID_JOINT_RZ;
+ values[0].id = ID_JOINT_RZ;
+ v_nr = 1;
+ break;
+ case Joint::TransX:
+ value[0].id = ID_JOINT_TX;
+ values[0].id = ID_JOINT_TX;
+ v_nr = 1;
+ break;
+ case Joint::TransY:
+ value[0].id = ID_JOINT_TY;
+ values[0].id = ID_JOINT_TY;
+ v_nr = 1;
+ break;
+ case Joint::TransZ:
+ value[0].id = ID_JOINT_TZ;
+ values[0].id = ID_JOINT_TZ;
+ v_nr = 1;
+ break;
+ case Joint::Sphere:
+ values[0].id = value[0].id = ID_JOINT_RX;
+ values[1].id = value[1].id = ID_JOINT_RY;
+ values[2].id = value[2].id = ID_JOINT_RZ;
+ v_nr = 3;
+ break;
+ case Joint::Swing:
+ values[0].id = value[0].id = ID_JOINT_RX;
+ values[1].id = value[1].id = ID_JOINT_RZ;
+ v_nr = 2;
+ break;
+ }
+}
+
+Armature::JointConstraint_struct::~JointConstraint_struct()
+{
+ if (freeParam && param)
+ free(param);
+}
+
void Armature::initCache(Cache *_cache)
{
m_cache = _cache;
@@ -65,6 +134,7 @@
if (m_cache) {
// add a special channel for the joint
m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
+#if 0
// for the constraints, instead of creating many different channels, we will
// create a single channel for all the constraints
if (m_nconstraint) {
@@ -72,8 +142,9 @@
m_buf = new double[m_nconstraint*constraintCacheSize];
}
// store the initial cache position at timestamp 0
+ pushConstraints(0);
+#endif
pushQ(0);
- pushConstraints(0);
}
}
@@ -103,7 +174,7 @@
}
return true;
}
-
+#if 0
void Armature::pushConstraints(CacheTS timestamp)
{
if (m_yCCh >= 0) {
@@ -132,7 +203,7 @@
if (item && m_yCTs != timestamp) {
for (unsigned int i=0; i<m_nconstraint; i++) {
JointConstraint_struct* pConstraint = m_constraints[i];
- if (pConstraint->function != JointLimitCallback) {
+ if (pConstraint->function != Joint1DOFLimitCallback) {
pConstraint->values.feedback = *item++;
pConstraint->values.tolerance = *item++;
pConstraint->value.yd = *item++;
@@ -148,6 +219,7 @@
}
return true;
}
+#endif
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)
{
@@ -219,7 +291,7 @@
{
SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
// not suitable for NDof joints
- if (segment_it == m_tree.getSegments().end() || segment_it->second.segment.getJoint().getNDof() != 1) {
+ if (segment_it == m_tree.getSegments().end()) {
if (_freeParam && _param)
free(_param);
return -1;
@@ -247,83 +319,226 @@
return -1;
}
// new constraint, append
- pConstraint = new JointConstraint_struct(segment_it, ID_JOINT, _function, _param, _freeParam, _substep);
+ pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep);
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].rest;
+ m_noutput += pConstraint->v_nr;
return m_nconstraint++;
}
-bool Armature::JointLimitCallback(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+bool Armature::Joint1DOFLimitCallback(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param)
{
// called from updateControlOutput() when a limit is set on a joint
// update the parameters
- LimitConstraintParam_struct* pLimit = (LimitConstraintParam_struct*)_param;
- double y = _values->values->y;
-#if 1
- if (y > pLimit->maxThreshold-0.001) {
- _values->alpha = pLimit->maxWeight;
+ Limit1DOFConstraintParam_struct* pLimit = (Limit1DOFConstraintParam_struct*)_param;
+ unsigned int numstep = (timestamp.numstep) ? timestamp.numstep : 4;
+ if (!timestamp.substep) {
+ _values[0].alpha = 0.0;
+ pLimit->timestep = timestamp.realTimestep/numstep;
+ _values[0].feedback = 1.0/pLimit->timestep;
+ pLimit->toggle = false;
+ return true;
+ } else if (pLimit->toggle) {
+ _values[0].alpha = 0.0;
+ pLimit->toggle = false;
+ return true;
+ }
+ double y = _values[0].values->y;
+ if (y > pLimit->maxThreshold+0.001) {
+ _values[0].alpha = pLimit->maxWeight;
// change the limit to the threshold value so that there is no oscillation
- _values->values->yd = pLimit->maxThreshold;
+ _values[0].values->yd = pLimit->maxThreshold;
} else if (y < pLimit->minThreshold-0.001) {
- _values->alpha = pLimit->maxWeight;
+ _values[0].alpha = pLimit->maxWeight;
// change the limit to the threshold value so that there is no oscillation
- _values->values->yd = pLimit->minThreshold;
+ _values[0].values->yd = pLimit->minThreshold;
} else {
- _values->alpha = 0.0;
+ _values[0].alpha = 0.0;
}
-#else
- // more complex formula to avoid discontinuity of velocity
- // not needed in animation, disable for now as it creates instability
- double x;
- if (y > pLimit->maxThreshold) {
- if (y < pLimit->max) {
- x = (pLimit->max-y)/pLimit->threshold;
- _values->alpha = pLimit->maxWeight*(1.0-x)/(1.0+pLimit->slope*x);
+ pLimit->toggle = !pLimit->toggle;
+ return true;
+}
+
+bool Armature::JointSwingLimitCallback(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+ // called from updateControlOutput() when a limit is set on a joint
+ // update the parameters
+ LimitSwingConstraintParam_struct* pLimit = (LimitSwingConstraintParam_struct*)_param;
+ double norm, qx, qz, CX, CZ, sx, sz;
+ unsigned int numstep = (timestamp.numstep) ? timestamp.numstep : 4;
+ if (!timestamp.substep) {
+ _values[0].alpha = 0.0;
+ _values[1].alpha = 0.0;
+ pLimit->timestep = timestamp.realTimestep/numstep;
+ _values[0].feedback = _values[1].feedback = 1.0/pLimit->timestep;
+ pLimit->toggle = false;
+ return true;
+ } else if (pLimit->toggle) {
+ _values[0].alpha = 0.0;
+ _values[1].alpha = 0.0;
+ pLimit->toggle = false;
+ return true;
+ }
+ switch (pLimit->limitType) {
+ case SWING_LIMIT_X:
+ qx = _values[0].values->y;
+ _values[1].alpha = 0.0;
+ if (qx > pLimit->maxX+0.001) {
+ _values[0].values->yd = pLimit->maxX;
+ _values[0].alpha = pLimit->maxWeight;
+ } else if (qx < pLimit->minX-0.001) {
+ _values[0].values->yd = pLimit->minX;
+ _values[0].alpha = pLimit->maxWeight;
} else {
- _values->alpha = pLimit->maxWeight;
+ _values[0].alpha = 0.0;
}
- // change the limit to the threshold value so that there is no oscillation
- _values->values->yd = pLimit->maxThreshold;
- } else if (y < pLimit->minThreshold) {
- if (y > pLimit->min) {
- x = (y-pLimit->min)/pLimit->threshold;
- _values->alpha = pLimit->maxWeight*(1.0-x)/(1.0+pLimit->slope*x);
+ break;
+
+ case SWING_LIMIT_Z:
+ qz = _values[1].values->y;
+ _values[0].alpha = 0.0;
+ if (qz > pLimit->maxZ+0.001) {
+ _values[1].values->yd = pLimit->maxZ;
+ _values[1].alpha = pLimit->maxWeight;
+ } else if (qz < pLimit->minZ-0.001) {
+ _values[1].values->yd = pLimit->minZ;
+ _values[1].alpha = pLimit->maxWeight;
} else {
- _values->alpha = pLimit->maxWeight;
+ _values[1].alpha = 0.0;
}
- // change the limit to the threshold value so that there is no oscillation
- _values->values->yd = pLimit->minThreshold;
- } else {
- _values->alpha = 0.0;
+ break;
+
+ case SWING_LIMIT_XZ:
+ qx = _values[0].values->y;
+ qz = _values[1].values->y;
+ sx = sz = 1.0;
+ // determine in which quadrant we are
+ if (qx > 0.0 && qz > 0.0) {
+ CX = pLimit->maxX;
+ CZ = pLimit->maxZ;
+ } else if (qx <= 0.0 && qz > 0.0) {
+ CX = -pLimit->minX;
+ CZ = pLimit->maxZ;
+ qx = -qx;
+ sx = -1.0;
+ } else if (qx <= 0.0 && qz <= 0.0) {
+ CX = -pLimit->minX;
+ CZ = -pLimit->minZ;
+ qx = -qx;
+ qz = -qz;
+ sx = sz = -1.0;
+ } else {
+ CX = pLimit->maxX;
+ CZ = -pLimit->minZ;
+ qz = -qz;
+ sz = -1.0;
+ }
+ if (CX < KDL::epsilon || CZ < KDL::epsilon) {
+ // quadrant is degenerated
+ if (qx > CX+0.001) {
+ _values[0].values->yd = CX*sx;
+ _values[0].alpha = pLimit->maxWeight;
+ } else {
+ _values[0].alpha = 0.0;
+ }
+ if (qz > CZ+0.001) {
+ _values[1].values->yd = CZ*sz;
+ _values[1].alpha = pLimit->maxWeight;
+ } else {
+ _values[1].alpha = 0.0;
+ }
+ } else {
+ // general case
+ qx /= CX;
+ qz /= CZ;
+ norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
+ if (norm > 1.001) {
+ norm = 1.0/norm;
+ _values[0].values->yd = qx*norm*CX*sx;
+ _values[1].values->yd = qz*norm*CZ*sz;
+ _values[0].alpha = pLimit->maxWeight;
+ _values[1].alpha = pLimit->maxWeight;
+ } else {
+ _values[0].alpha = 0.0;
+ _values[1].alpha = 0.0;
+ }
+ }
}
-#endif
+ pLimit->toggle = !pLimit->toggle;
return true;
}
-int Armature::addLimitConstraint(const std::string& segment_name, double _min, double _max, double _threshold, double _maxWeight, double _slope)
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list