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. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
http://lists.blender.org/mailman/listinfo/bf-blender-cvs
