[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22432] branches/itasc: iTaSC: add support for Distance IK constraint.
Benoit Bolsee
benoit.bolsee at online.be
Thu Aug 13 17:13:44 CEST 2009
Revision: 22432
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22432
Author: ben2610
Date: 2009-08-13 17:13:44 +0200 (Thu, 13 Aug 2009)
Log Message:
-----------
iTaSC: add support for Distance IK constraint. The 'on surface' variant is implemented. The inside and outside variants will be implemented later. It is working nicely as long as the surface is reachable. Unreachable surface produce jitter in the armature due to ineffective damping in the solver. Will try to find a solution.
Modified Paths:
--------------
branches/itasc/intern/itasc/Distance.cpp
branches/itasc/intern/itasc/Solver.hpp
branches/itasc/intern/itasc/WSDLSSolver.hpp
branches/itasc/intern/itasc/kdl/chainjnttojacsolver.cpp
branches/itasc/intern/itasc/kdl/chainjnttojacsolver.hpp
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/itasc/intern/itasc/Distance.cpp
===================================================================
--- branches/itasc/intern/itasc/Distance.cpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/intern/itasc/Distance.cpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -21,9 +21,9 @@
m_chiKdl(6),m_jac(6),m_cache(NULL),
m_distCCh(-1),m_distCTs(0)
{
- m_chain.addSegment(Segment(Joint(Joint::RotY)));
+ m_chain.addSegment(Segment(Joint(Joint::RotZ)));
m_chain.addSegment(Segment(Joint(Joint::RotX)));
- m_chain.addSegment(Segment(Joint(Joint::TransZ)));
+ m_chain.addSegment(Segment(Joint(Joint::TransY)));
m_chain.addSegment(Segment(Joint(Joint::RotZ)));
m_chain.addSegment(Segment(Joint(Joint::RotY)));
m_chain.addSegment(Segment(Joint(Joint::RotX)));
@@ -40,6 +40,7 @@
memset(&m_data, 0, sizeof(m_data));
// initialize the data with normally fixed values
m_data.id = ID_DISTANCE;
+ m_values.id = ID_DISTANCE;
m_values.number = 1;
m_values.alpha = m_alpha;
m_values.feedback = m_K;
@@ -63,20 +64,20 @@
m_chi(0) = 0.0;
m_chi(1) = 0.0;
} else {
- // find the XY angles that bring the Z axis to point to init_pose.p
+ // find the XZ angles that bring the Y axis to point to init_pose.p
Vector axis(pose.p/dist);
- alpha = 0.0;
- if (fabs(axis(1)) > 1-KDL::epsilon) {
- // direction is aligned on X axis, just rotation on Y
- beta = 0.0;
- gamma = -KDL::sign(axis(1))*KDL::PI/2;
+ beta = 0.0;
+ if (fabs(axis(2)) > 1-KDL::epsilon) {
+ // direction is aligned on Z axis, just rotation on X
+ alpha = 0.0;
+ gamma = KDL::sign(axis(2))*KDL::PI/2;
} else {
- beta = KDL::atan2(axis(0), axis(2));
- gamma = -KDL::atan2(axis(1), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(2))));
+ alpha = -KDL::atan2(axis(0), axis(1));
+ gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
}
// rotation after first 2 joints
basis = Rotation::EulerZYX(alpha, beta, gamma);
- m_chi(0) = beta;
+ m_chi(0) = alpha;
m_chi(1) = gamma;
}
m_chi(2) = dist;
Modified: branches/itasc/intern/itasc/Solver.hpp
===================================================================
--- branches/itasc/intern/itasc/Solver.hpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/intern/itasc/Solver.hpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -21,6 +21,7 @@
// size of vector = nc, alternance of true / false to indicate the grouping of output
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0;
virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0;
+ virtual void setQmax(double _qmax) {}
};
}
Modified: branches/itasc/intern/itasc/WSDLSSolver.hpp
===================================================================
--- branches/itasc/intern/itasc/WSDLSSolver.hpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/intern/itasc/WSDLSSolver.hpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -26,7 +26,7 @@
virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc);
virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
- void setQmax(double _qmax){m_qmax=_qmax;};
+ virtual void setQmax(double _qmax){m_qmax=_qmax;};
};
Modified: branches/itasc/intern/itasc/kdl/chainjnttojacsolver.cpp
===================================================================
--- branches/itasc/intern/itasc/kdl/chainjnttojacsolver.cpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/intern/itasc/kdl/chainjnttojacsolver.cpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -36,29 +36,44 @@
{
assert(q_in.rows()==chain.getNrOfJoints()&&
q_in.rows()==jac.columns());
- T_tmp = Frame::Identity();
- SetToZero(t_tmp);
- int j=0;
- Frame total;
- // this could be done much better by walking from the ee to the base
- for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
+
+
+ Frame T_local, T_joint;
+ T_total = Frame::Identity();
+ SetToZero(t_local);
+
+ int i=chain.getNrOfSegments()-1;
+ unsigned int q_nr = chain.getNrOfJoints();
+
+ //Lets recursively iterate until we are in the root segment
+ while (i >= 0) {
const Segment& segment = chain.getSegment(i);
int ndof = segment.getJoint().getNDof();
- //Calculate new Frame_base_ee
- total = T_tmp*segment.pose(((JntArray&)q_in)(j));
- //Changing Refpoint of all columns to new ee
- changeRefPoint(jac,total.p-T_tmp.p,jac);
+ q_nr -= ndof;
+ //get the pose of the joint.
+ T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+ // combine with the tip to have the tip pose
+ T_local = T_joint*segment.getFrameToTip();
+ //calculate new T_end:
+ T_total = T_local * T_total;
+
for (int dof=0; dof<ndof; dof++) {
- //pose of the new end-point expressed in the base
- //changing base of new segment's twist to base frame
- t_tmp = T_tmp.M*segment.twist(total.p,1.0,dof);
- jac.twists[j+dof] = t_tmp;
- }
- j+=ndof;
-
- T_tmp = total;
- }
+ // combine joint rotation with tip position to get a reference frame for the joint
+ T_joint.p = T_local.p;
+ // in which the twist can be computed (needed for NDof joint)
+ t_local = segment.twist(T_joint, 1.0, dof);
+ //transform the endpoint of the local twist to the global endpoint:
+ t_local = t_local.RefPoint(T_total.p - T_local.p);
+ //transform the base of the twist to the endpoint
+ t_local = T_total.M.Inverse(t_local);
+ //store the twist in the jacobian:
+ jac.twists[q_nr+dof] = t_local;
+ }
+ i--;
+ }//endwhile
+ //Change the base of the complete jacobian from the endpoint to the base
+ changeBase(jac, T_total.M, jac);
return 0;
}
}
Modified: branches/itasc/intern/itasc/kdl/chainjnttojacsolver.hpp
===================================================================
--- branches/itasc/intern/itasc/kdl/chainjnttojacsolver.hpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/intern/itasc/kdl/chainjnttojacsolver.hpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -57,8 +57,8 @@
private:
const Chain chain;
- Twist t_tmp;
- Frame T_tmp;
+ Twist t_local;
+ Frame T_total;
};
}
#endif
Modified: branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-08-13 14:27:35 UTC (rev 22431)
+++ branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-08-13 15:13:44 UTC (rev 22432)
@@ -36,8 +36,10 @@
#include "MovingFrame.hpp"
#include "CopyPose.hpp"
#include "WSDLSSolver.hpp"
+#include "WDLSSolver.hpp"
#include "Scene.hpp"
#include "Cache.hpp"
+#include "Distance.hpp"
#include "MEM_guardedalloc.h"
@@ -127,7 +129,7 @@
iTaSC::Scene* scene;
iTaSC::MovingFrame* base; // armature base object
KDL::Frame baseFrame; // frame of armature base relative to blArmature
- iTaSC::WSDLSSolver* solver;
+ iTaSC::Solver* solver;
Object* blArmature;
struct bConstraint* polarConstraint;
@@ -531,7 +533,7 @@
return true;
}
-static bool constraint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
{
IK_Target* iktarget =(IK_Target*)_param;
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
@@ -556,6 +558,24 @@
return true;
}
+static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+ IK_Target* iktarget =(IK_Target*)_param;
+ bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
+ iTaSC::ConstraintValues* values = _values;
+ bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
+ // we need default parameters
+ if (!ikparam)
+ ikparam = &DefIKParam;
+
+ values->alpha = (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) ? 0.0 : condata->weight;
+ values->values[0].yd = condata->dist;
+ values->values[0].action = iTaSC::ACT_VALUE;
+ values->feedback = ikparam->feedback;
+ values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK;
+ return true;
+}
+
static IK_Scene* convert_tree(Object *ob, bPoseChannel *pchan)
{
PoseTree *tree = (PoseTree*)pchan->iktree.first;
@@ -588,6 +608,7 @@
if (!ingame)
ikscene->cache = new iTaSC::Cache();;
ikscene->solver = new iTaSC::WSDLSSolver();
+ //ikscene->solver = new iTaSC::WDLSSolver();
//ikscene->solver->setQmax(10.0);
ikscene->blArmature = ob;
@@ -952,33 +973,49 @@
for (a=0, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next, a++) {
condata= (bKinematicConstraint*)target->con->data;
pchan = tree->pchan[target->tip];
+ unsigned int controltype;
// add the end effector
IK_Target* iktarget = ikscene->targets[a];
- unsigned int controltype = 0;
- if ((condata->flag & CONSTRAINT_IK_ROT) && (condata->orientweight != 0.0))
- controltype |= iTaSC::CopyPose::CTL_ROTATION;
- if ((condata->weight != 0.0))
- controltype |= iTaSC::CopyPose::CTL_POSITION;
- if (controltype) {
- iktarget->constraint = new iTaSC::CopyPose(controltype, controltype);
+ switch (condata->type) {
+ case CONSTRAINT_IK_COPYPOSE:
+ controltype = 0;
+ if ((condata->flag & CONSTRAINT_IK_ROT) && (condata->orientweight != 0.0))
+ controltype |= iTaSC::CopyPose::CTL_ROTATION;
+ if ((condata->weight != 0.0))
+ controltype |= iTaSC::CopyPose::CTL_POSITION;
+ if (controltype) {
+ iktarget->constraint = new iTaSC::CopyPose(controltype, controltype);
+ iktarget->blenderConstraint = target->con;
+ // set the gain
+ if (controltype & iTaSC::CopyPose::CTL_POSITION)
+ iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
+ if (controltype & iTaSC::CopyPose::CTL_ROTATION)
+ iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list