[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22959] branches/itasc: NDoF joint limit properly implemented.

Benoit Bolsee benoit.bolsee at online.be
Thu Sep 3 01:00:52 CEST 2009


Revision: 22959
          http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22959
Author:   ben2610
Date:     2009-09-03 01:00:52 +0200 (Thu, 03 Sep 2009)

Log Message:
-----------
NDoF joint limit properly implemented. Use same method as old solver: lock offending joints at the limit and reiterate for the same substep. More CPU but more stable. Joint limit is not included in the constraint output anymore, which is then available for other type of constraint (e.g. action).

Modified Paths:
--------------
    branches/itasc/intern/itasc/Armature.cpp
    branches/itasc/intern/itasc/Armature.hpp
    branches/itasc/intern/itasc/ControlledObject.hpp
    branches/itasc/intern/itasc/Scene.cpp
    branches/itasc/intern/itasc/Scene.hpp
    branches/itasc/intern/itasc/eigen_types.hpp
    branches/itasc/intern/itasc/kdl/utilities/utility.cpp
    branches/itasc/intern/itasc/kdl/utilities/utility.h
    branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp

Modified: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp	2009-09-02 22:16:00 UTC (rev 22958)
+++ branches/itasc/intern/itasc/Armature.cpp	2009-09-02 23:00:52 UTC (rev 22959)
@@ -32,6 +32,7 @@
 	m_yCTs(0),
 	m_qKdl(),
 	m_oldqKdl(),
+	m_newqKdl(),
 	m_qdotKdl(),
 	m_jac(NULL),
 	m_jacsolver(NULL),
@@ -231,7 +232,7 @@
 		return false;
 	int ndof = joint.getNDof();
 	for (int dof=0; dof<ndof; dof++) {
-		Joint_struct js((&q_rest)[dof], joint.getType(), ndof);
+		Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
 		m_joints.push_back(js);
 	}
 	m_njoint+=ndof;
@@ -325,150 +326,8 @@
 	return m_nconstraint++;
 }
 
-bool Armature::Joint1DOFLimitCallback(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max)
 {
-	// called from updateControlOutput() when a limit is set on a joint
-	// update the parameters
-	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[0].values->yd = pLimit->maxThreshold;
-	} else if (y < pLimit->minThreshold-0.001) {
-		_values[0].alpha = pLimit->maxWeight;
-		// change the limit to the threshold value so that there is no oscillation
-		_values[0].values->yd = pLimit->minThreshold;
-	} else {
-		_values[0].alpha = 0.0;
-	}
-	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[0].alpha = 0.0;
-		}
-		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[1].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;
-			}
-		}
-	}
-	pLimit->toggle = !pLimit->toggle;
-	return true;
-}
-
-int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max, double _threshold, double _maxWeight)
-{
 	SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
 	if (segment_it == m_tree.getSegments().end())
 		return -1;
@@ -483,62 +342,12 @@
 		// for rotation joint, the limit is given in degree, convert to radian
 		_min *= KDL::deg2rad;
 		_max *= KDL::deg2rad;
-		_threshold *= KDL::deg2rad;
 	}
-	if (joint.getType() == Joint::Swing) {
-		// check if a constraint is already defined and if so, update it
-		JointConstraintList::iterator constraint_it;
-		JointConstraint_struct* pConstraint;
-		int iConstraint;
-		for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
-			pConstraint = *constraint_it;
-			if (pConstraint->segment == segment_it) {
-				if (pConstraint->function != JointSwingLimitCallback)
-					break;
-				LimitSwingConstraintParam_struct* param = (LimitSwingConstraintParam_struct*)pConstraint->param;
-				switch (dof) {
-				case 0:
-					param->minX = (_min < -_threshold) ? _min+_threshold : 0.0;
-					param->maxX = (_max >  _threshold) ? _max-_threshold : 0.0;
-					param->limitType |= SWING_LIMIT_X;
-					break;
-				case 1:
-					param->minZ = (_min < -_threshold) ? _min+_threshold : 0.0;;
-					param->maxZ = (_max >  _threshold) ? _max-_threshold : 0.0;
-					param->limitType |= SWING_LIMIT_Z;
-					break;
-				}
-				return iConstraint;
-			}
-		}
-		// new constraint
-		LimitSwingConstraintParam_struct* param = new LimitSwingConstraintParam_struct;
-		memset(param, 0, sizeof(*param));
-		param->threshold = _threshold;
-		param->maxWeight = _maxWeight;
-		switch (dof) {
-		case 0:
-			param->limitType = SWING_LIMIT_X;
-			param->minX = _min;
-			param->maxX = _max;
-			break;
-		case 1:
-			param->limitType = SWING_LIMIT_Z;
-			param->minZ = _min;
-			param->maxZ = _max;
-			break;
-		}
-		return addConstraint(segment_name, JointSwingLimitCallback, param, true, true);
-	} else {
-		Limit1DOFConstraintParam_struct* param = new Limit1DOFConstraintParam_struct;
-		param->max = _max;
-		param->min = _min;
-		param->threshold = _threshold;
-		param->maxThreshold = _max - _threshold;
-		param->minThreshold = _min + _threshold;
-		param->maxWeight = _maxWeight;
-		return addConstraint(segment_name, Joint1DOFLimitCallback, param, true, true);
-	}
+	Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
+	p_joint.min = _min;
+	p_joint.max = _max;
+	p_joint.useLimit = true;
+	return 0;
 }
 
 int Armature::addEndEffector(const std::string& name)
@@ -578,9 +387,10 @@
 	m_jac = new Jacobian(m_njoint);
 	m_qKdl.resize(m_njoint);
 	m_oldqKdl.resize(m_njoint);
+	m_newqKdl.resize(m_njoint);
 	m_qdotKdl.resize(m_njoint);
 	for (i=0; i<m_njoint; i++) {
-		m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
+		m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
 	}
 	updateJacobian();
 	// estimate the maximum size of the robot arms
@@ -610,41 +420,192 @@
 	}
 }
 
-void Armature::updateKinematics(const Timestamp& timestamp){
-
-    //TODO: what about the timestep?
-    //Integrate m_qdot
+bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
+{
 	if (!m_finalized)
-		return;
-
-	// integration, for spherical joint we must use a more sophisticated method
+		return false;
+	
+	// integration and joint limit
+	// for spherical joint we must use a more sophisticated method
 	unsigned int q_nr;
+	double* qdot=&m_qdotKdl(0);
+	double* q=&m_qKdl(0);
+	double* newq=&m_newqKdl(0);
+	double norm, qx, qz, CX, CZ, sx, sz;
+	bool locked = false;
+	int unlocked = 0;
+
 	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::Swing:
+	for (q_nr=0; q_nr<m_nq; ) {
+		Joint_struct* joint = &m_joints[q_nr];
+		if (!joint->locked) {
+			switch (joint->type) {
+			case KDL::Joint::Swing:
 			{
-				double* qdot=&m_qdotKdl(q_nr);
-				double* q=&m_qKdl(q_nr);
-				(KDL::Rot(KDL::Vector(q[0],0.0,q[1]))*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(q);
+				KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
+				(base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
+				if (joint[0].useLimit) {
+					if (joint[1].useLimit) {
+						// elliptical limit
+						sx = sz = 1.0;
+						qx = newq[0];
+						qz = newq[1];
+						// determine in which quadrant we are
+						if (qx > 0.0 && qz > 0.0) {
+							CX = joint[0].max;
+							CZ = joint[1].max;
+						} else if (qx <= 0.0 && qz > 0.0) {
+							CX = -joint[0].min;
+							CZ = joint[1].max;
+							qx = -qx;
+							sx = -1.0;
+						} else if (qx <= 0.0 && qz <= 0.0) {
+							CX = -joint[0].min;
+							CZ = -joint[1].min;
+							qx = -qx;
+							qz = -qz;
+							sx = sz = -1.0;
+						} else {
+							CX = joint[0].max;
+							CZ = -joint[0].min;
+							qz = -qz;
+							sz = -1.0;
+						}
+						if (CX < KDL::epsilon || CZ < KDL::epsilon) {
+							// quadrant is degenerated
+							if (qx > CX) {
+								newq[0] = CX*sx;
+								joint[0].locked = true;
+							}
+							if (qz > CZ) {
+								newq[1] = CZ*sz;
+								joint[0].locked = true;
+							}
+						} else {
+							// general case

@@ Diff output truncated at 10240 characters. @@




More information about the Bf-blender-cvs mailing list