[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20853] branches/ge_dev: iTaSC update:
Benoit Bolsee
benoit.bolsee at online.be
Sat Jun 13 14:29:49 CEST 2009
Revision: 20853
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20853
Author: ben2610
Date: 2009-06-13 14:29:46 +0200 (Sat, 13 Jun 2009)
Log Message:
-----------
iTaSC update:
Fix bug with MovingFrame velocity not being reset when the cache is empty.
iTaSC API: Allow in-place modification of Joint weights.
Detect end of first timestep iterations also by no movements on end effectors.
Always use even number of substeps in automatic substepping to avoid oscillation effect.
Fix bug in armature: implement rest pose in fixed joints rather that in non-zero joint angles.
Support joints stiffness and joints limit (not tested yet).
Modified Paths:
--------------
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Armature.hpp
branches/ge_dev/intern/itasc/ControlledObject.hpp
branches/ge_dev/intern/itasc/MovingFrame.cpp
branches/ge_dev/intern/itasc/Scene.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-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -191,6 +191,25 @@
return maxJoint;
}
+double Armature::getMaxEndEffectorChange()
+{
+ if (!m_finalized)
+ return 0.0;
+ double maxDelta = 0.0;
+ double delta;
+ Twist twist;
+ for (unsigned int i = 0; i<m_neffector; i++) {
+ twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
+ delta = twist.rot.Norm();
+ if (delta > maxDelta)
+ maxDelta = delta;
+ delta = twist.vel.Norm();
+ if (delta > maxDelta)
+ maxDelta = delta;
+ }
+ return maxDelta;
+}
+
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);
@@ -414,6 +433,9 @@
if (!timestamp.substep) {
// save previous joint state for getMaxJointChange()
memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
+ for (unsigned int i=0; i<m_neffector; i++) {
+ m_effectors[i].oldpose = m_effectors[i].pose;
+ }
}
JointConstraintList::iterator it;
Modified: branches/ge_dev/intern/itasc/Armature.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.hpp 2009-06-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/intern/itasc/Armature.hpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -27,6 +27,7 @@
// 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 getMaxJointChange();
+ double getMaxEndEffectorChange();
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);
@@ -42,8 +43,9 @@
struct Effector_struct {
std::string name;
+ Frame oldpose;
Frame pose;
- Effector_struct(const std::string& _name) {name = _name; pose = F_identity;}
+ Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
};
typedef std::vector<Effector_struct> EffectorList;
Modified: branches/ge_dev/intern/itasc/ControlledObject.hpp
===================================================================
--- branches/ge_dev/intern/itasc/ControlledObject.hpp 2009-06-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/intern/itasc/ControlledObject.hpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -41,7 +41,7 @@
virtual const e_matrix& getCq() const{return m_Cq;};
- virtual const e_matrix& getWq() const {return m_Wq;};
+ virtual e_matrix& getWq() {return m_Wq;};
virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;};
virtual const e_vector& getWy() const {return m_Wy;};
Modified: branches/ge_dev/intern/itasc/MovingFrame.cpp
===================================================================
--- branches/ge_dev/intern/itasc/MovingFrame.cpp 2009-06-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/intern/itasc/MovingFrame.cpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -106,6 +106,7 @@
} else {
// first position in cache, no velocity as we cannot interpolate
m_internalPose = m_nextPose;
+ m_velocity = Twist::Zero();
m_xudot = e_zero_vector(6);
// recompute the jacobian
updateJacobian();
Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp 2009-06-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/intern/itasc/Scene.cpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -398,7 +398,8 @@
cs->task->getMaxTimestep(timesubstep);
}
// use substep that are dividers of timestep for more regularity
- timesubstep = timestep/(1.0+floor((timestep/timesubstep)-0.333));
+ maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666);
+ timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep);
if (timesubstep >= timeleft-(m_minstep/2.0)) {
timesubstep = timeleft;
numsubstep = 1;
Modified: branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-13 11:52:33 UTC (rev 20852)
+++ branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-13 12:29:46 UTC (rev 20853)
@@ -486,6 +486,8 @@
std::string joint;
std::string root("root");
std::string parent;
+ std::vector<double> weights;
+ double weight[3];
// assume uniform scaling and take Y scale as general scale for the armature
float scale = VecLength(ob->obmat[1]);
double X, Y, Z;
@@ -523,7 +525,7 @@
XY ==> RX+RY(tip)
XZ ==> RX+RZ(tip)
YZ ==> RZ+RY(tip)
- XYZ ==> RX+RZ+RY(tip)
+ XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
In case of stretch, tip=(0,0,0) and there is an additional TY joint
The frame at last of these joints represents the tail of the bone.
The head is computed by a reverse translation on Y axis of the bone length
@@ -539,17 +541,17 @@
*/
KDL::Frame tip(iTaSC::F_identity);
Vector3 *fl = bone->bone_mat;
- KDL::Rotation bonerot(
+ KDL::Frame head(KDL::Rotation(
fl[0][0], fl[1][0], fl[2][0],
fl[0][1], fl[1][1], fl[2][1],
- fl[0][2], fl[1][2], fl[2][2]);
-
+ fl[0][2], fl[1][2], fl[2][2]),
+ KDL::Vector(bone->head[0], bone->head[1], bone->head[2]));
+
// take scaling into account
length= bone->length*scale;
parent = (a > 0) ? ikscene->channels[tree->parent[a]].bone : root;
// first the fixed segment to the bone head
- if (VecLength(bone->head) > KDL::epsilon) {
- KDL::Frame head(KDL::Vector(bone->head[0], bone->head[1], bone->head[2]));
+ if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
joint = bone->name;
joint += ":H";
ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
@@ -560,6 +562,9 @@
tip.p[1] = length;
}
joint = bone->name;
+ weight[0] = 1.0/(1.0-pchan->stiffness[0]);
+ weight[1] = 1.0/(1.0-pchan->stiffness[1]);
+ weight[2] = 1.0/(1.0-pchan->stiffness[2]);
switch (flag)
{
case 0:
@@ -569,85 +574,103 @@
break;
case IK_XDOF:
// RX only, get the X rotation
- X = EulerAngleFromMatrix(bonerot, 0);
+ //X = EulerAngleFromMatrix(bonerot, 0);
joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, X, tip);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, 0.0, tip);
+ weights.push_back(weight[0]);
break;
case IK_YDOF:
// RY only, get the Y rotation
- Y = ComputeTwist(bonerot);
+ //Y = ComputeTwist(bonerot);
joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y, tip);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, 0.0, tip);
+ weights.push_back(weight[1]);
break;
case IK_ZDOF:
- // RX only, get the X rotation
- Z = EulerAngleFromMatrix(bonerot, 2);
+ // RZ only, get the Zz rotation
+ //Z = EulerAngleFromMatrix(bonerot, 2);
joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, Z, tip);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, 0.0, tip);
+ weights.push_back(weight[2]);
break;
case IK_XDOF|IK_YDOF:
- Y = ComputeTwist(bonerot);
- RemoveEulerAngleFromMatrix(bonerot, Y, 1);
- X = EulerAngleFromMatrix(bonerot, 0);
+ //Y = ComputeTwist(bonerot);
+ //RemoveEulerAngleFromMatrix(bonerot, Y, 1);
+ //X = EulerAngleFromMatrix(bonerot, 0);
joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, X);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, 0.0);
+ weights.push_back(weight[0]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, 0.0);
+ weights.push_back(weight[1]);
}
break;
case IK_XDOF|IK_ZDOF:
// RX+RZ
- Z = EulerAngleFromMatrix(bonerot, 2);
- RemoveEulerAngleFromMatrix(bonerot, Z, 2);
- X = EulerAngleFromMatrix(bonerot, 0);
+ //Z = EulerAngleFromMatrix(bonerot, 2);
+ //RemoveEulerAngleFromMatrix(bonerot, Z, 2);
+ //X = EulerAngleFromMatrix(bonerot, 0);
joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, X);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, 0.0);
+ weights.push_back(weight[0]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, Z);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, 0.0);
+ weights.push_back(weight[2]);
}
break;
case IK_YDOF|IK_ZDOF:
// RZ+RY
- Y = ComputeTwist(bonerot);
- RemoveEulerAngleFromMatrix(bonerot, Y, 1);
- Z = EulerAngleFromMatrix(bonerot, 2);
+ //Y = ComputeTwist(bonerot);
+ //RemoveEulerAngleFromMatrix(bonerot, Y, 1);
+ //Z = EulerAngleFromMatrix(bonerot, 2);
joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, Z);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, 0.0);
+ weights.push_back(weight[2]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, 0.0);
+ weights.push_back(weight[1]);
}
break;
case IK_XDOF|IK_YDOF|IK_ZDOF:
// spherical joint
- KDL::Vector rot = bonerot.GetRot();
- joint += ":SJ";
- ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot(0), tip);
- /*
- GetEulerXYZ(bonerot, X, Y, Z);
- joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, X);
- if (ret) {
- parent = joint;
- joint = bone->name;
- joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y);
+ if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT)) {
+ // cannot use spherical joint in this case, decompose in several joints
+ //GetEulerXZY(bonerot, X, Z, Y);
+ joint += ":RX";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, 0.0);
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list