KChainOrientationBodySchema Class Reference

This class is a wrapper for KinematicChain, so that it becomes a BodySchema. More...

#include <KChainOrientationBodySchema.h>

Inheritance diagram for KChainOrientationBodySchema:
KChainBodySchema BodySchema KinematicChain

Public Member Functions

 KChainOrientationBodySchema (int teles)
void Angle2Cart (joint_vec_t &angles, cart_vec_t &pos)
void Angle2Cart (cart_vec_t &pos)
int InverseKinematics (const cart_vec_t &pos, joint_vec_t &angles)
 samples the workspace to find adequate weights
void Jacobian (Matrix &jac)
void AddVirtualRotationAxis ()
int GetNbJoints ()
int GetNbJoints () const
virtual int InverseKinematics (const cart_vec_t &pos, joint_vec_t &start_angle, joint_vec_t &angles)
virtual float TryInverseKinematics (const cart_vec_t &pos, joint_vec_t &angles)
virtual void SetRandomAngle (joint_vec_t &angles)
virtual void Jacobian (joint_vec_t &angles, Matrix &jac)
virtual void Load (istream &in)
virtual void Load (const char *filename)
virtual void Print (ostream &out) const
virtual void GetWeights (cart_vec_t &cart_weights, joint_vec_t &joint_weights)
 samples the workspace to find adequate weights
virtual void SetPosition (joint_vec_t &angles)
virtual void GetAngles (joint_vec_t &angles)
void GetAngles (float *angles)
 Get the rotation angles of all joints.
virtual float * Serialize (float *data, int offset)
 serializes a kinematic chain into a float array
virtual int Deserialize (const float *data, int offset)
 reads a kinematic chain from a float array
virtual int Deserialize (const float *data, int from, int to)
 reads a kinematic chain from a float array
void SetIkTrials (int n)
virtual void FullJacobian (Matrix &jac)
 full jacobian for position and orientation
virtual void GetRandomAngle (joint_vec_t &angles)
virtual void SetAnglesInRange (joint_vec_t &angle)
float AngleInClosestRange (int i, float cand)
 param angle assumed to be in [-pi pi]
virtual void UpdateDimension ()
virtual int AnglesInRange (joint_vec_t &angles)
virtual void GetAnglesLowerBound (joint_vec_t &lb)
virtual joint_vec_t & GetAnglesLowerBound ()
virtual void GetAnglesUpperBound (joint_vec_t &ub)
virtual joint_vec_t & GetAnglesUpperBound ()
virtual void SetAnglesLowerBound (joint_vec_t &lb)
virtual void SetAnglesUpperBound (joint_vec_t &ub)
void SetAngleBounds (joint_vec_t &lb, joint_vec_t &ub)
virtual float CartesianDistance (cart_vec_t &p1, cart_vec_t &p2)
void FreeChain ()
 Deallocated memory for rotations and translations.
int SetLink (int i, CVector3_t link)
 Sets the translation vector of a given link.
void AddLastLink (Translation *trans)
 Adds a last link to the chain (so that it end up with a Translation).
void SetLastLink (const CVector3_t llink)
 Sets the value of the last translation.
TranslationGetLastLink () const
int SetAllLinks (CVector3_t *links)
int SetRotationAxis (int i, CVector3_t axis)
 Sets the rotation axis of a given joint.
void SetRotationAxis (int i, float x, float y, float z)
 Sets the rotation axis of a given joint.
int GetRotationAxis (int i, CVector3_t axis)
 Get the rotation axis of a given joint.
float GetAngle (int i)
 Get the rotation angle of a given joint.
void SetAngles (float *angles)
 Set the rotation angles of all joints.
void SetAngle (int i, float angle)
 Set the rotation angle of a given joint.
int SetAllRotationAxes (CVector3_t *axes)
 Set the rotation axes of all joints.
void ScaleRate (float factor)
 scales all the learning rates
void SetForwardKinematics (float *angles, CVector3_t pos, CVector3_t local_pos=NULL)
 Computes the forward kinematics for a given angle configuration the desired angle configuration.
void ForwardKinematics (CVector3_t pos, CVector3_t local_pos=NULL) const
 Computes the actual position of the end-effector.
float InverseKinPosCCD (CVector3_t pos)
 CCD inverse kinematics function for position constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.
void InverseKinRotCCD (CQuat_t q)
 Performs CCD inverse kinematics function for orientation constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.
float InverseKinematicsCCD (CVector3_t pos, CQuat_t rot)
 Performs CCD inverse kinematics function for position and orientation constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.
void GetJacobian (float *jac)
 Computes the position jacobian of the kinematic chain.
void GetFullJacobian (float *jac)
 Computes the full (position and orientation) jacobian of the kinematic chain.
float Update (float *angles, CVector3_t vision, CVector3_t local_pos=NULL)
 Updates the kinematic chain rotation axes and translation vectors using static updating algo see Hersch et al.
float Update (float *angles, CVector3_t vision, float threshold, CVector3_t local_pos=NULL)
 Updates the kinematic chain rotation axes and translation vectors using static updating algo see Hersch et al.
float UpdateWithJacobian (float *angle_pos, float *angle_disp, CVector3_t visual_disp, float threshold, CVector3_t local_pos=NULL)
 Updates the kinematic chain rotation axes and translation vectors using kinetic updating algo, by comparing and joint angle displacement with the corresponding visual displacement.
float UpdateWithFullJacobian (float *angle_pos, float *angle_disp, RigidTransfo *visual_disp)
 Updates the kinematic chain rotation axes using kinetic updating algo, by comparing and joint angle displacement with the corresponding visual rotation.
float UpdateWithFullJacobian (float *angle_pos, float *angle_disp, float *visual_disp)
void LinkRef (float *angle, int n, CVector3_t in, CVector3_t out)
int GetLink (int i, CVector3_t link)
RigidTransfoGetTransfo (int i) const
int AddJoint (RigidTransfo *t, int inv=1)
void Clear ()
void RandomAngles (float *angles)
void RandomAxes ()
void SetRandomAngles ()
modality_t GetModality () const
void SetModality (modality_t mod)
void Transform (int i, CVector3_t in, CVector3_t out) const
 transforms a vector with the ith transformation (or link), index starts at zero
void Transform (int i, CVector3_t in, float angle, CVector3_t out)
 transforms a vector with the ith transformation (or link), index starts at zero.
void Rotate (int i, CVector3_t in, CVector3_t out) const
void CounterRotate (int i, CVector3_t in, CVector3_t out) const
void InverseTransform (int i, CVector3_t in, CVector3_t out) const
void Translate (int i, CVector3_t in, CVector3_t out) const
void GetTranslation (int i, CVector3_t t)
void GetQuaternion (int i, CQuat_t q) const
void GetInverseQuaternion (int i, CQuat_t q) const
void QuaternionAxisAngleDerivative (int i, CMatrix4_t m)
void QuaternionAxisDerivative (int i, CMatrix4_t m)
void QuaternionAngleDerivative (int i, CQuat_t q)
int IsInverted (int i) const
void Invert (int i)
void InvertAll ()
int Copy (const KinematicChain &kc)
int RemoveJoint ()
void DeleteJoint ()
float GetTolerance () const
void SetTolerance (float t)
void GlobalTransfo (RigidTransfo &rt)
 computes the composed of all rigid transformation of the chain
void GlobalRotation (Rotation &rot)
 computes the composed of all rotations of the chain
void GlobalTranslation (CVector3_t trans)
virtual float CollisionDetection (float &l1, float &l2)
 checks for a collision between the two extremal links of the chain
float UpdateTouch (float *angle, float k0, float k1)
 updates the kinematic chain based on touch information.
float ScaleChain (float len)
void PrintAxes (ostream &out)

Protected Member Functions

float TryInverseKinematics (const cart_vec_t)
void RotationStack (CQuat_t *qstack)
 q1[i] contains all rotation up to i-1 i.e.
void ForwardKinematicsStack (CVector3_t pos, CVector3_t *pstack)
 computes the position of a point in the frames of reference of all joints.
void InverseKinematicsStack (CVector3_t pos, CVector3_t *pstack)
 computes the position of a point in the frames of reference of all joints.
void InitKinematicStack (const CVector3_t local_pos, CVector3_t out) const
 computes the value to feed into ForwardKinematicStack (depending on ifdef WITH_LAST_LINK)

Protected Attributes

float rot_tol
joint_vec_t min_angle
 joint angle lower limits
joint_vec_t max_angle
 joint angle upper limits
int ik_trials
RigidTransfojoints_block [MAX_LINKS]
 used of code optimization purposes
RigidTransfo ** joints
 An array of pointer to rigid transformation representing the links and joints of a kinematic chain.
int reverse_block [MAX_LINKS]
int * reverse
 if reverse[i] == -1 means that this rigid transfo is to be inverted.
int nb_joints
Translationlast_link
int own_last_link
modality_t modality
float tol

Detailed Description

This class is a wrapper for KinematicChain, so that it becomes a BodySchema.

Definition at line 27 of file KChainOrientationBodySchema.h.


Member Function Documentation

void KinematicChain::AddLastLink ( Translation trans  )  [inline, inherited]

Adds a last link to the chain (so that it end up with a Translation).

Parameters:
trans a pointer to the translation to add

Definition at line 142 of file KinematicChain.h.

Referenced by KinematicTree::LoadChain().

00142                                           {last_link=trans;}
00143     {if(last_link)delete last_link;last_link=trans;own_last_link =0;} //to avoid memory leak

float KinematicChain::CollisionDetection ( float &  l1,
float &  l2 
) [virtual, inherited]

checks for a collision between the two extremal links of the chain

checks for a collision between the two extremal links of the chain.

Does not include the last link

Definition at line 443 of file KinematicChain.cpp.

References KinematicChain::GlobalRotation(), KinematicChain::GlobalTransfo(), KinematicChain::joints, Rotation::Transform(), and RigidTransfo::Transform().

00443                                                             {
00444   float distance,d=0.0f;
00445   int last = nb_joints-1;
00446   RigidTransfo rt;
00447   Rotation rot;
00448   CVector3_t v,v1,v2,p1,p2;
00449   v_clear(v);
00450   for(int i=1;i<last;i++){
00451     d += v_squ_length(joints[i]->GetTranslation());
00452   }
00453   if(d<epsilon){
00454     return -1;
00455   }
00456   GlobalTransfo(rt);
00457   GlobalRotation(rot);
00458   v_clear(p1);
00459   rt.Transform(p1,p2);
00460   if(IsInverted(last)==1){
00461     v_scale(GetTransfo(last)->GetTranslation(),-1,v);
00462   }
00463   else{
00464     GetTransfo(last)->GetTranslation(v);
00465   }
00466   rot.Transform(v,v2);
00467   //  cout<<"transfo "<<v<<" "<<v2<<" "<<v_length(v)<<" "<<v_length(v2)<<endl;
00468   
00469   if(IsInverted(0)==-1){
00470     v_scale(GetTransfo(0)->GetTranslation(),-1,v1);
00471   }
00472   else{
00473     GetTransfo(0)->GetTranslation(v1);
00474   }
00475 //   cout<<"input"<<endl<<p1<<" "<<v1<<endl;
00476 //   cout<<p2<<" "<<v2<<endl;
00477 //   cout<<*this<<endl;
00478   if(segments_nearest_points(p1,v1,p2,v2,&l1,&l2,&distance)){
00479     return distance;  
00480   }
00481   else{
00482     return -1;
00483   }
00484 }

virtual int KChainBodySchema::Deserialize ( const float *  data,
int  from,
int  to 
) [inline, virtual, inherited]

reads a kinematic chain from a float array

Parameters:
from where to start reading in the data
to where to finish reading in the data
Returns:
the number of joints read

Reimplemented from BodySchema.

Definition at line 64 of file KChainBodySchema.h.

References KChainBodySchema::Deserialize().

00065     {return KinematicChain::Deserialize(data,from,to);};

virtual int KChainBodySchema::Deserialize ( const float *  data,
int  offset 
) [inline, virtual, inherited]

reads a kinematic chain from a float array

Parameters:
offset joint with which to start (for partial kinematic chain updating)

Reimplemented from BodySchema.

Definition at line 62 of file KChainBodySchema.h.

Referenced by KChainBodySchema::Deserialize().

00063     {return KinematicChain::Deserialize(data,offset);};

void KinematicChain::ForwardKinematics ( CVector3_t  pos,
CVector3_t  local_pos = NULL 
) const [inherited]

Computes the actual position of the end-effector.

Parameters:
pos where to put the result the point in the frame of reference of the end-effector, which position we want to compute. Default is zero

Definition at line 187 of file KinematicChain.cpp.

References KinematicChain::Transform().

Referenced by KinematicChain::SetForwardKinematics().

00187                                                                                {
00188     CVector3_t v1,v2;
00189   int i;
00190 #ifdef WITH_LAST_LINK
00191   if(!local_pos){
00192       v_clear(v2); 
00193       last_link->Transform(v2,v1);
00194   }
00195   else{
00196       last_link->Transform(local_pos,v1);
00197   }
00198 #else
00199   if(!local_pos){
00200       v_clear(v1);
00201   }
00202   else{
00203      v_copy(local_pos,v1);
00204   }
00205 #endif 
00206  for(i=nb_joints-1;i>=0;i--){
00207     Transform(i,v1,v2);
00208     v_copy(v2,v1);
00209   }
00210   v_copy(v1,pos);
00211 }

void KinematicChain::ForwardKinematicsStack ( CVector3_t  pos,
CVector3_t *  pstack 
) [protected, inherited]

computes the position of a point in the frames of reference of all joints.

v1[i] contains the value after it was transformed by joint i,i.e in the frame of ref of joint i-1

Parameters:
pos the position of the point in the frame of reference of the most distal joint (the last_link if there is one)
pstack where to put all the results. After the call pstack[i] contains the value after it was transformed by joint i,i.e in the frame of ref of joint i-1

Definition at line 400 of file KinematicChain.cpp.

References KinematicChain::Transform().

Referenced by KinematicChain::GetFullJacobian(), KinematicChain::GetJacobian(), KChainBodySchema::GetWeights(), KinematicChain::Update(), KinematicChain::UpdateTouch(), and KinematicChain::UpdateWithJacobian().

00400                                                                         {
00401   v_copy(pos,v1[nb_joints]);
00402   for(int i=nb_joints;i>0;i--){
00403     Transform(i-1,v1[i],v1[i-1]);
00404  }
00405 }

float KinematicChain::GetAngle ( int  i  )  [inherited]

Get the rotation angle of a given joint.

Parameters:
i index of the joint(assumed to be >= 0)
Returns:
the rotation angle in rad

Definition at line 142 of file KinematicChain.cpp.

References KinematicChain::joints.

Referenced by KinematicChain::GetAngles().

00142                                    {
00143   if(i>=0 && i< nb_joints){
00144     return joints[i]->GetRotationAngle();
00145   }
00146   else{
00147     cout<<"no such joint: "<<i<<endl;
00148     return 0.0;
00149   }
00150 }

void KinematicChain::GetAngles ( float *  angles  )  [inline, inherited]

Get the rotation angles of all joints.

Parameters:
angles where to put to the angles (in rad)

Definition at line 187 of file KinematicChain.h.

References KinematicChain::GetAngle().

00188     {for(int i=0;i<nb_joints;i++)angles[i]=GetAngle(i);}

void KinematicChain::GetFullJacobian ( float *  jac  )  [inherited]

Computes the full (position and orientation) jacobian of the kinematic chain.

Parameters:
jac the matrix where to put the jacobian. Its size is 6*n, where n is the number of joints the chain. The numbers are arranged as jac[0]=dx0/dth0,jac[1]=dx1/dth0,jac[2]=dx2/dth0... jac[6]= dx0/dth1,... So columns start every six elements. The first three rows correspond to the position and last three correspond to the orientation, using the complex part of the quaternion. See Hersch et al (2008), "Iterative rigid body transformation estimation for visual tracking",VISAPP08 for more details on this representation

Definition at line 679 of file KinematicChain.cpp.

References KinematicChain::ForwardKinematicsStack(), Rotation::InverseNormDerivative(), KinematicChain::joints, Rotation::NormDerivative(), Rotation::QuaternionNormDerivative(), KinematicChain::reverse, KinematicChain::RotationStack(), Rotation::SetQuaternion(), and Rotation::Transform().

Referenced by KinematicChain::UpdateWithFullJacobian().

00679                                                {
00680   CQuat_t q[MAX_LINKS];
00681   CVector3_t v1[MAX_LINKS];
00682   CVector3_t v2,der; 
00683   CQuat_t q1,q2,q3,q4;
00684   float f;
00685   Rotation rot;
00686 
00687 #ifdef WITH_LAST_LINK
00688   last_link->GetTranslation(v2);
00689 #else
00690   v_clear(v2);
00691 #endif
00692   q_clear(q1);
00693   RotationStack(q);
00694   ForwardKinematicsStack(v2,v1);
00695   for(int i=nb_joints-1;i>=0;i--){
00696     rot.SetQuaternion(q[i]);
00697     joints[i]->NormDerivative(v1[i],v2);
00698     if(reverse[i]==-1){
00699       joints[i]->InverseNormDerivative(v1[i+1],v2);
00700       joints[i]->InverseQuaternionNormDerivative(q3);
00701     }
00702     else{
00703       joints[i]->NormDerivative(v1[i+1],v2);
00704       joints[i]->QuaternionNormDerivative(q3);
00705     }
00706     rot.Transform(v2,der);//d/dsin(theta/2);
00707     f=0.5*joints[i]->GetAlpha();
00708     v_scale(der,f,jac+6*i);
00709     q_multiply(q1,q3,q4);
00710     q_multiply(q4,q[i],q3);//q3 = q[i]* q2'* q1 where '*': quat. mult
00711     v_scale(q3,f,jac+6*i+3);//we take only the 3 first components
00712     GetQuaternion(i,q2);  
00713     q_multiply(q1,q2,q3); //for the next iteration
00714     q_copy(q3,q1);
00715  }
00716 }

void KinematicChain::GetJacobian ( float *  jac  )  [inherited]

Computes the position jacobian of the kinematic chain.

Parameters:
jac the matrix where to put the jacobian. Its size is 4*n, where n is the number of joints the chain. The numbers are arranged as jac[0]=dx0/dth0,jac[1]=dx1/dth0,jac[2]=dx2/dth0... jac[4]= dx0/dth1,... So columns start every four elements and the last element of each column (i.e. jac[3], jac[7]..) are not as x has only 3 component. those elements are there for historical reasons.

Definition at line 645 of file KinematicChain.cpp.

References KinematicChain::ForwardKinematicsStack(), Rotation::InverseNormDerivative(), KinematicChain::joints, Rotation::NormDerivative(), KinematicChain::reverse, KinematicChain::RotationStack(), Rotation::SetQuaternion(), and Rotation::Transform().

00645                                            {
00646   CQuat_t q[MAX_LINKS];
00647   CVector3_t v1[MAX_LINKS];
00648   CVector3_t v2,der; 
00649   float f;
00650   Rotation rot;
00651 #ifdef WITH_LAST_LINK
00652   last_link->GetTranslation(v2);
00653 #else
00654   v_clear(v2);//modify for reaching with specific point
00655 #endif
00656   RotationStack(q);
00657   ForwardKinematicsStack(v2,v1);
00658 #ifdef WITH_LAST_LINK
00659   for(int i=0;i<nb_joints;i++)//{ below
00660 #else
00661   for(int i=0;i<nb_joints-1;i++)//{ below ;  assuming the last rotation is irrelevant for the position
00662 #endif    
00663     { 
00664    rot.SetQuaternion(q[i]);
00665     if(reverse[i]==-1){
00666       joints[i]->InverseNormDerivative(v1[i+1],v2);
00667     }
00668     else{
00669       joints[i]->NormDerivative(v1[i+1],v2);
00670     }
00671     rot.Transform(v2,der);//d/dsin(theta/2);
00672     f=0.5*joints[i]->GetAlpha();
00673     v_scale(der,f,jac+4*i);
00674     jac[4*i+3]=0;
00675   }
00676 }

int KinematicChain::GetNbJoints (  )  const [inline, inherited]
Returns:
the number of joints in the chain

Definition at line 128 of file KinematicChain.h.

Referenced by ArticulatedTree::FindAngleRanges(), KinematicTree::LoadChainRec(), and KinematicChain::Serialize().

00128 {return nb_joints;};

int KinematicChain::GetRotationAxis ( int  i,
CVector3_t  axis 
) [inherited]

Get the rotation axis of a given joint.

Parameters:
i joint index (assumed to be >= 0)
axis where to put the rotation axis

Definition at line 135 of file KinematicChain.cpp.

References KinematicChain::joints.

Referenced by KinematicChain::Serialize().

00135                                                         {
00136   if(i>=nb_joints){
00137     return 0;
00138   }
00139   return joints[i]->GetRotationAxis(axis);
00140 }

void KinematicChain::GlobalRotation ( Rotation rot  )  [inherited]

computes the composed of all rotations of the chain

Parameters:
rot where to put the resulting rotation

Definition at line 424 of file KinematicChain.cpp.

References KinematicChain::joints, and KinematicChain::reverse.

Referenced by KinematicChain::CollisionDetection(), and KinematicChain::GlobalTransfo().

00424                                               {
00425   int i;
00426   r.Identity();
00427   for(i=0;i<nb_joints;i++){
00428     if(reverse[i] == -1){
00429       joints[i]->Invert();
00430       r = r*((Rotation&)(*(joints[i])));
00431       joints[i]->Invert();   
00432     }
00433     else{
00434       r = r*((Rotation&)(*(joints[i])));
00435     }
00436   }
00437 }

void KinematicChain::GlobalTransfo ( RigidTransfo rt  )  [inherited]

computes the composed of all rigid transformation of the chain

Parameters:
rot where to put the resulting transformation

Definition at line 417 of file KinematicChain.cpp.

References KinematicChain::GlobalRotation().

Referenced by KinematicChain::CollisionDetection().

00417                                                   {
00418   CVector3_t transl;
00419   GlobalTranslation(transl);
00420   GlobalRotation(rt);
00421   rt.SetTranslation(transl);
00422 }

float KinematicChain::InverseKinematicsCCD ( CVector3_t  pos,
CQuat_t  rot 
) [inherited]

Performs CCD inverse kinematics function for position and orientation constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.

Hersch) thesis for a description of the algorithm

Parameters:
pos the target position
rot the target orientation, more precisely the quaternion describing the desired total rotation of the end-effector
Returns:
distance to target position

Definition at line 323 of file KinematicChain.cpp.

References KinematicChain::InverseKinematicsStack(), Rotation::InvertRotationAngle(), and KinematicChain::joints.

00323                                                                      {
00324   int i,j;
00325   float rdist=0,tdist=0,k=0.001; //rotation vs translation weight
00326   CVector3_t stack[MAX_LINKS];
00327   CVector3_t tar,newrod,rod,diff;
00328   CQuat_t q1,q2,q3,q4;
00329 
00330   q_inv(rot,q1);
00331   for(i=0;i<nb_joints;i++){
00332     GetQuaternion(i,q2);
00333     q_multiply(q2,q1,q3);
00334     q_copy(q3,q1);
00335   } 
00336   for(j=0;j<50;j++){
00337 #ifdef WITH_LAST_LINK
00338     last_link->GetTranslation(rod);
00339 #else
00340     v_clear(rod);
00341 #endif
00342     InverseKinematicsStack(pos,stack);
00343     for(i=nb_joints-1;i>=0;i--){
00344       GetInverseQuaternion(i,q2);
00345       q_multiply(q2,q1,q3); //q1*inv(Ri)
00346       if(IsInverted(i)==-1){
00347         v_copy(stack[i],tar);
00348         Translate(i,rod,newrod); //getting to joint
00349         joints[i]->MinimizePositionAndRotation(tar,newrod,q3,k);
00350         joints[i]->InvertRotationAngle();
00351         Rotate(i,newrod,rod);   // updating "rod"
00352         v_sub(tar,rod,diff);
00353       }
00354       else{
00355         Rotate(i,stack[i+1],tar); //rotating back
00356         joints[i]->MinimizePositionAndRotation(tar,rod,q3,k);
00357         Rotate(i,rod,newrod);
00358         Translate(i,newrod,rod);
00359         v_sub(tar,newrod,diff);
00360       }
00361       GetQuaternion(i,q2);
00362       q_multiply(q3,q2,q1);
00363       q_multiply(q2,q3,q4);
00364       rdist = v_length(q4);//rotation distance, only the first 3 components 
00365       tdist = v_length(diff);//translation distance
00366       //     cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
00367       if(tdist<tol && rdist<0.05){return rdist/0.05+tdist;}
00368     }
00369     q_multiply(rot,q1,q2);
00370     q_inv(rot,q3);
00371     q_multiply(q2,q3,q1);
00372   }
00373   return rdist/0.05 + tdist;
00374 }

void KinematicChain::InverseKinematicsStack ( CVector3_t  pos,
CVector3_t *  ikstack 
) [protected, inherited]

computes the position of a point in the frames of reference of all joints.

ikstack[i] contains the values after it was inverse transformed by joints up to i-1, i.e.

Parameters:
pos the position of the point in the frame of reference of the world
pstack where to put all the results. After the call ikstack[i] contains the values after it was inverse transformed by joints up to i-1, i.e. in the frame of ref of joint i-1

in the frame of ref of joint i-1

Definition at line 410 of file KinematicChain.cpp.

Referenced by KinematicChain::InverseKinematicsCCD(), and KinematicChain::InverseKinPosCCD().

00410                                                                               {
00411   v_copy(pos,ikstack[0]);
00412   for(int i=0;i<nb_joints;i++){
00413     InverseTransform(i,ikstack[i],ikstack[i+1]);
00414   }
00415 }

float KinematicChain::InverseKinPosCCD ( CVector3_t  pos  )  [inherited]

CCD inverse kinematics function for position constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.

CCD inverse kinematics function for position constraints.

Hersch) thesis for a description of the algorithm

Parameters:
pos the target position
Returns:
distance to target position
distance to target position

Definition at line 255 of file KinematicChain.cpp.

References Rotation::AimAt(), KinematicChain::InverseKinematicsStack(), Rotation::InvertRotationAngle(), and KinematicChain::joints.

00255                                                     {
00256   CVector3_t stack[MAX_LINKS];
00257   CVector3_t tar,newrod,rod,diff;
00258   float dist=-1.0f;
00259   for(int j=0;j<50;j++){
00260     InverseKinematicsStack(pos,stack);
00261 #ifdef WITH_LAST_LINK
00262     last_link->GetTranslation(rod);
00263 #else
00264     v_clear(rod);//modify for reaching with specific points
00265 #endif
00266     for(int i=nb_joints-1;i>=0;i--){
00267       if(IsInverted(i)==-1){
00268         v_copy(stack[i],tar);
00269         Translate(i,rod,newrod); //getting to joint
00270         joints[i]->AimAt(tar,newrod);
00271         joints[i]->InvertRotationAngle();
00272         Rotate(i,newrod,rod);   // updating "rod"
00273         v_sub(tar,rod,diff);
00274       }
00275       else{
00276         Rotate(i,stack[i+1],tar); //rotating back
00277         joints[i]->AimAt(tar,rod);
00278         Rotate(i,rod,newrod);
00279         Translate(i,newrod,rod);
00280         v_sub(tar,newrod,diff);
00281       }
00282       dist=v_length(diff);
00283       if(dist<=tol){return dist;}
00284       //     cout<<v_length(diff)<<endl;;
00285     }
00286   }
00287   return dist;
00288 }

void KinematicChain::InverseKinRotCCD ( CQuat_t  q  )  [inherited]

Performs CCD inverse kinematics function for orientation constraints Performs Cyclic Coordinate Descent global inverse kinematics (see Wang and Chen (1991) or my (M.

Hersch) thesis for a description of the algorithm. The formula is slightly different from the original, it was derived differently.

Parameters:
q the target orientation, more precisely the quaternion describing the desired total rotation of the end-effector

Definition at line 293 of file KinematicChain.cpp.

References Rotation::InvertRotationAngle(), KinematicChain::joints, and Rotation::MinimizeRotation().

00293                                               {
00294   int i,j;
00295   CQuat_t q1,q2,q3,q4;
00296   
00297   q_inv(q,q1);
00298   for(i=0;i<nb_joints;i++){ //should be nb_joints-1 if not using last_link
00299     GetQuaternion(i,q2);
00300     q_multiply(q2,q1,q3);
00301     q_copy(q3,q1);
00302   } 
00303   for(j=0;j<20;j++){
00304     for(i=nb_joints-1;i>=0;i--){//should be nb_joints-2 if not using last_link
00305       GetInverseQuaternion(i,q2);
00306       q_multiply(q2,q1,q3); //q1*inv(Ri)
00307       joints[i]->MinimizeRotation(q3);
00308       if(IsInverted(i)==-1){
00309         joints[i]->InvertRotationAngle();
00310       }
00311       GetQuaternion(i,q2);
00312       q_multiply(q3,q2,q1);
00313       q_multiply(q2,q3,q4);
00314       cout<<"rot "<<v_length(q4)<<endl;
00315       //      if(v_length(diff)<tol){return;}
00316     }
00317     q_multiply(q,q1,q2);
00318     q_inv(q,q3);
00319     q_multiply(q2,q3,q1);
00320   }
00321 }

void KinematicChain::RotationStack ( CQuat_t *  q1  )  [protected, inherited]

q1[i] contains all rotation up to i-1 i.e.

q1*q2*q3*qi-1

Definition at line 383 of file KinematicChain.cpp.

References KinematicChain::joints, and KinematicChain::reverse.

Referenced by KinematicChain::GetFullJacobian(), KinematicChain::GetJacobian(), KinematicChain::Update(), KinematicChain::UpdateTouch(), and KinematicChain::UpdateWithJacobian().

00383                                              {
00384   CQuat_t q;
00385   q_clear(q1[0]);//
00386   for(int i=0;i<nb_joints;i++){
00387     joints[i]->GetQuaternion(q);
00388     if(reverse[i]==-1){
00389       v_scale(q,-1,q); //inverting only the first 3 comp
00390     }
00391     q_multiply(q,q1[i],q1[i+1]);
00392   }
00393 }

virtual float* KChainBodySchema::Serialize ( float *  data,
int  offset 
) [inline, virtual, inherited]

serializes a kinematic chain into a float array

Parameters:
offset joint with which to start (for partial kinematic chain serialization)

Reimplemented from BodySchema.

Definition at line 61 of file KChainBodySchema.h.

00061 {return KinematicChain::Serialize(data,offset);};

int KinematicChain::SetAllRotationAxes ( CVector3_t *  axes  )  [inherited]

Set the rotation axes of all joints.

Parameters:
axes a pointer to an array of CVector3_t. Is assumed to be of the proper length (i.e nb_joints)

Definition at line 120 of file KinematicChain.cpp.

References KinematicChain::SetRotationAxis().

00120                                                       {
00121   int ret =1;
00122   for(int i=0;i<nb_joints;i++){
00123     ret*= SetRotationAxis(i,axes[i]);
00124   } 
00125   return ret;
00126 }

void KinematicChain::SetAngle ( int  i,
float  angle 
) [inline, inherited]

Set the rotation angle of a given joint.

Parameters:
i index of the joint(assumed to be >= 0)
angle the desired angle in rad

Definition at line 201 of file KinematicChain.h.

References KinematicChain::joints, and Rotation::SetAngle().

00201 {joints[i]->SetAngle(angle);}

void KinematicChain::SetAngles ( float *  angles  )  [inherited]

Set the rotation angles of all joints.

Parameters:
all desired angles (in rad)

Definition at line 220 of file KinematicChain.cpp.

References KinematicChain::joints, and Rotation::SetAngle().

Referenced by KinematicChain::SetForwardKinematics(), KinematicChain::Update(), KinematicChain::UpdateTouch(), KinematicChain::UpdateWithFullJacobian(), and KinematicChain::UpdateWithJacobian().

00220                                            {
00221   for(int i=0;i<nb_joints;i++){
00222       joints[i]->SetAngle(angles[i]);
00223   }
00224 }

void KinematicChain::SetForwardKinematics ( float *  angles,
CVector3_t  pos,
CVector3_t  local_pos = NULL 
) [inline, inherited]

Computes the forward kinematics for a given angle configuration the desired angle configuration.

Parameters:
pos where to put the result the point in the frame of reference of the end-effector, which position we want to compute. Default is zero

Definition at line 222 of file KinematicChain.h.

References KinematicChain::ForwardKinematics(), and KinematicChain::SetAngles().

00222                                                                                        {
00223         SetAngles(angles);ForwardKinematics(pos,local_pos);};

int KinematicChain::SetLink ( int  i,
CVector3_t  link 
) [inherited]

Sets the translation vector of a given link.

Parameters:
i link index (assumed to be > 0)
i link index (assumed to be > 0)

Definition at line 83 of file KinematicChain.cpp.

References KinematicChain::joints.

00083                                                  {
00084   if(i==nb_joints){
00085 #ifdef WITH_LAST_LINK
00086     last_link->SetTranslation(link);
00087 #endif
00088   }
00089   else{
00090     if(i>nb_joints){
00091       return 0;
00092     }
00093     else{
00094       joints[i]->SetTranslation(link);
00095     }
00096   }
00097   return 1;
00098 }

void KinematicChain::SetRotationAxis ( int  i,
float  x,
float  y,
float  z 
) [inline, inherited]

Sets the rotation axis of a given joint.

Parameters:
i joint index (assumed to be >= 0)

Definition at line 166 of file KinematicChain.h.

References KinematicChain::SetRotationAxis().

Referenced by KinematicChain::SetRotationAxis().

00167     {CVector3_t a;v_set(x,y,z,a);SetRotationAxis(i,a);};

int KinematicChain::SetRotationAxis ( int  i,
CVector3_t  axis 
) [inherited]

Sets the rotation axis of a given joint.

Parameters:
i joint index (assumed to be >= 0)

Definition at line 113 of file KinematicChain.cpp.

References KinematicChain::joints.

Referenced by KinematicChain::SetAllRotationAxes().

00113                                                         {
00114   if(i>=nb_joints){
00115     return 0;
00116   }
00117   return joints[i]->SetRotationAxis(axis);
00118 }

void KinematicChain::Transform ( int  i,
CVector3_t  in,
float  angle,
CVector3_t  out 
) [inherited]

transforms a vector with the ith transformation (or link), index starts at zero.

Sets the rotation angle prior to performing the rotation.

Parameters:
i the index of the transformation to consider
in the vector to be transform
angle the rotation angle
out where to put the result

Definition at line 214 of file KinematicChain.cpp.

References KinematicChain::joints, Rotation::SetAngle(), and KinematicChain::Transform().

00214                                                                              {
00215   joints[i]->SetAngle(angle);
00216   Transform(i,in,out);
00217 }

void KinematicChain::Transform ( int  i,
CVector3_t  in,
CVector3_t  out 
) const [inline, inherited]

transforms a vector with the ith transformation (or link), index starts at zero

Parameters:
i the index of the transformation to consider
in the vector to be transform
out where to put the result

Definition at line 371 of file KinematicChain.h.

References RigidTransfo::InverseTransform(), KinematicChain::joints, KinematicChain::reverse, and RigidTransfo::Transform().

Referenced by KinematicChain::ForwardKinematics(), KinematicChain::ForwardKinematicsStack(), and KinematicChain::Transform().

00372     {if(reverse[i]==-1){joints[i]->InverseTransform(in,out);}else{joints[i]->Transform(in,out);}}

float KinematicChain::Update ( float *  angles,
CVector3_t  vision,
float  threshold,
CVector3_t  local_pos = NULL 
) [inherited]

Updates the kinematic chain rotation axes and translation vectors using static updating algo see Hersch et al.

(2008) IJHR

Parameters:
angles the configuration of the kinematic chain
vision the visual position of the end-effector
threshold the maximal distance between seen and predicted position of the end-effector for the update to take place. Used to discard outliers in the vision data
local_pos the position of the marker with respect to the end-effector
Returns:
the distance between the seen and the predicted end-effector position. If the sign is negative this distance is above the threshold and the chain was not updated (bad visual tracking assumed).

Definition at line 1146 of file KinematicChain.cpp.

References Rotation::AxisDerivative(), KinematicChain::ForwardKinematicsStack(), KinematicChain::InitKinematicStack(), Rotation::InverseAxisDerivative(), KinematicChain::joints, KinematicChain::reverse, KinematicChain::RotationStack(), KinematicChain::SetAngles(), Rotation::SetQuaternion(), and Rotation::Transform().

01146                                                                                                   {
01147   CVector3_t v1[MAX_LINKS], delta[MAX_LINKS], delta_tr[MAX_LINKS];
01148   CVector3_t v,v2;
01149   //  CQuat_t q; unused
01150   CQuat_t q1[MAX_LINKS];
01151   CMatrix3_t m2,m3;
01152   Rotation rot;
01153   int i;
01154   float sum=0.0f,sum_tr=0.0f,ret_val;
01155   
01156   SetAngles(angle);
01157   InitKinematicStack(local_pos,v2);
01158   ForwardKinematicsStack(v2,v1);
01159   v_sub(vision,v1[0],v); //(y-R(0))
01160   ret_val = v_length(v);//return value
01161   if(ret_val > threshold){
01162       return -ret_val;
01163   }
01164   // stop learning if ok
01165   //  if (ret_val<1){return ret_val;}
01166 
01167 
01168   // computing the composed of all rotations
01169   RotationStack(q1);
01170   
01171   // starting updating process
01172   for(i=0;i<nb_joints;i++){
01173     //    for(i=0;i<2;i++){
01174     if(!joints[i]->IsAdaptive()){
01175       v_clear(delta[i]);
01176       v_clear(delta_tr[i]);
01177     }
01178     else{
01179       if(reverse[i]==-1){
01180         rot.SetQuaternion(q1[i+1]);//including Ri in rotation
01181       }
01182       else{
01183         rot.SetQuaternion(q1[i]);
01184       }
01185        
01186       //         cout<<"rot "<<i<<" "<<rot<<endl;
01187       /*------------- updating translation -----------------*/
01188       m_identity(m2);
01189       // computing rotation matrix (should be equivalent to rot.RotToMatrix(m3))
01190       rot.Transform((m2+0),(m3+0));
01191       rot.Transform((m2+4),(m3+4));
01192       rot.Transform((m2+8),(m3+8));
01193       v_transform_normal2(v,m3,delta_tr[i]);
01194 
01195       //joints[i]->Translation::Add(delta_tr[i]);
01196       sum_tr+= v_squ_length(delta_tr[i]);
01197 
01198       /*-------------   updating rotation -----------------*/
01199       if(reverse[i]==-1){
01200         joints[i]->Translation::InverseTransform(v1[i+1],v2);//including transl. in transfo
01201         joints[i]->InverseAxisDerivative(v2,m2);
01202         rot.SetQuaternion(q1[i]); //exluding Ri from rotation
01203       }
01204       else{
01205         joints[i]->AxisDerivative(v1[i+1],m2);
01206       }
01207       rot.Transform((m2+0),(m3+0));
01208       rot.Transform((m2+4),(m3+4));
01209       rot.Transform((m2+8),(m3+8));
01210 
01211       v_transform_normal2(v,m3,delta[i]);
01212       //     cout<<"addedl "<<delta[i]<<endl;
01213       //      cout<<"mat"<<endl<<m2<<endl;
01214       //    joints[i]->Rotation::AddToAxis(delta[i]);
01215 
01216       sum+=v_squ_length(delta[i]);
01217 
01218       if(reverse[i] == -1){
01219         //       v_scale(delta[i],-1,delta[i]);
01220         v_scale(delta_tr[i],-1,delta_tr[i]);
01221       }
01222 
01223     }
01224   }
01225 #ifdef WITH_LAST_LINK
01226   // updating last link
01227   rot.SetQuaternion(q1[nb_joints]);
01228   //     rot.Transform((CVector3_t)(m2+0), (CVector3_t)(m3+0));
01229   m_identity(m2);
01230   rot.Transform((m2+0),(m3+0));
01231   rot.Transform((m2+4),(m3+4));
01232   rot.Transform((m2+8),(m3+8));
01233   v_transform_normal2(v,m3,delta_tr[nb_joints]);
01234   //  last_link->Add(delta_tr[nb_joints]);
01235   sum_tr+=v_squ_length(delta_tr[nb_joints]);
01236 #endif
01237   if(sum>epsilon){
01238     sum = 1/sqrt(sum);
01239     //       sum=0;
01240   }
01241   if(sum_tr>epsilon){
01242     sum_tr = 30/sqrt(sum_tr);//was 50
01243   }
01244   for(i=0;i<nb_joints;i++){
01245     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01246     v_scale(delta[i],sum,delta[i]);
01247     joints[i]->Translation::Add(delta_tr[i]);
01248     joints[i]->Rotation::AddToAxis(delta[i]);
01249     //      cout<<"added "<<delta[i]<<endl;     
01250   }
01251 #ifdef WITH_LAST_LINK
01252   v_scale(delta_tr[nb_joints],sum_tr,delta_tr[nb_joints]);
01253   last_link->Add(delta_tr[nb_joints]);
01254 #endif
01255   return ret_val;
01256 }

float KinematicChain::Update ( float *  angles,
CVector3_t  vision,
CVector3_t  local_pos = NULL 
) [inline, inherited]

Updates the kinematic chain rotation axes and translation vectors using static updating algo see Hersch et al.

(2008) IJHR

Parameters:
angles the configuration of the kinematic chain
vision the visual position of the end-effector
local_pos the position of the marker with respect to the end-effector
Returns:
the distance between the seen and the predicted end-effector position.

Definition at line 299 of file KinematicChain.h.

References KinematicChain::Update().

Referenced by KinematicChain::Update().

00300     {return Update(angles,vision,FLT_MAX,local_pos);};

float KinematicChain::UpdateTouch ( float *  angle,
float  k0,
float  k1 
) [inherited]

updates the kinematic chain based on touch information.

We assume that the links touch when their distance is below a threshold. But the sensors have to depts or direction.

Parameters:
angle the array of angles of the chain
k0 the touching point on the first segment
k1 the touching point on the last segment
Returns:
the distance between the two touching points in the current body schema

Definition at line 1409 of file KinematicChain.cpp.

References Rotation::AxisDerivative(), KinematicChain::ForwardKinematicsStack(), Rotation::InverseAxisDerivative(), KinematicChain::joints, KinematicChain::reverse, KinematicChain::RotationStack(), Rotation::RotToMatrix(), KinematicChain::SetAngles(), Rotation::SetQuaternion(), and Rotation::Transform().

01409                                                                  {
01410 
01411   CVector3_t kinstack[MAX_LINKS], delta[MAX_LINKS], delta_tr[MAX_LINKS];
01412   CVector3_t v,v2,touch_pos0,touch_pos1;
01413   //  CQuat_t q;
01414   CQuat_t q1[MAX_LINKS];
01415   CMatrix3_t m2,m3;
01416   //  RigidTransfo **joints_bk=NULL;
01417   //   int * reverse_bk;
01418   Rotation rot;
01419   int i;
01420   float sum=0.0f,sum_tr=0.0f,ret_val;
01421   int shifted =0;
01422   joints[nb_joints-1]->GetTranslation(touch_pos1);
01423   v_scale(touch_pos1,k1,touch_pos1);
01424   joints[0]->GetTranslation(touch_pos0);
01425   v_scale(touch_pos0,k0,touch_pos0);
01426 
01427   SetAngles(angle);
01428   nb_joints--; //discarding the last rotation
01429   
01430    if(IsInverted(0)==-1){
01431      // joints_bk = joints;   
01432      //   joints = &(joints[1]); //discarding the first rotation (shifting)
01433      // reverse_bk = reverse;
01434      //  reverse = &(reverse[1]);  
01435      joints++;
01436      reverse++;
01437      nb_joints--;
01438      shifted=1;
01439   }
01440  
01441 
01442 
01443   // forward kinematics
01444   ForwardKinematicsStack(touch_pos1,kinstack);
01445  
01446   //difference between real and current transfo
01447   v_sub(touch_pos0,kinstack[0],v); //(y-R(0)
01448  
01449   ret_val = v_length(v);//return value
01450   // stop learning if ok
01451   if (ret_val<10){
01452   // computing the composed of all rotations
01453   RotationStack(q1);
01454   for(i=0;i<nb_joints;i++){
01455     //    for(i=0;i<2;i++){
01456     if(!joints[i]->IsAdaptive()){
01457       v_clear(delta[i]);
01458       v_clear(delta_tr[i]);
01459     }
01460     else{
01461       if(reverse[i]==-1){
01462         rot.SetQuaternion(q1[i+1]);//including Ri in rotation
01463       }
01464       else{
01465         rot.SetQuaternion(q1[i]);
01466       }
01467        
01468       /*------------- updating translation -----------------*/
01469   
01470       // computing rotation matrix (should be equivalent to rot.InverseTransform(v,delta_tr))
01471       rot.RotToMatrix(m3);
01472       v_transform_normal2(v,m3,delta_tr[i]);
01473     
01474       if(reverse[i] == -1){ // to be checked
01475         v_scale(delta_tr[i],-1,delta_tr[i]);
01476       }    
01477       sum_tr+= v_squ_length(delta_tr[i]);
01478 
01479       /*-------------   updating rotation -----------------*/
01480 
01481       if(reverse[i]==-1){
01482         joints[i]->Translation::InverseTransform(kinstack[i+1],v2);//including transl. in transfo
01483         joints[i]->InverseAxisDerivative(v2,m2);
01484         rot.SetQuaternion(q1[i]); //exluding Ri from rotation
01485       }
01486       else{
01487         joints[i]->AxisDerivative(kinstack[i+1],m2);
01488       }
01489 
01490       rot.Transform((m2+0),(m3+0));
01491       rot.Transform((m2+4),(m3+4));
01492       rot.Transform((m2+8),(m3+8));
01493       v_transform_normal2(v,m3,delta[i]);
01494 
01495       sum+=v_squ_length(delta[i]);
01496     }
01497   }
01498   if(sum>epsilon){
01499     sum = 1/sqrt(sum);
01500   }
01501   if(sum_tr>epsilon){
01502     sum_tr = 30/sqrt(sum_tr);//was 50
01503   }
01504   for(i=0;i<nb_joints;i++){
01505     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01506     v_scale(delta[i],sum,delta[i]);
01507     joints[i]->Translation::Add(delta_tr[i]);
01508     joints[i]->Rotation::AddToAxis(delta[i]);
01509   }
01510   }
01511   nb_joints++;
01512   if(shifted){ //shifting back
01513     // joints = joints_bk;
01514     //    reverse = reverse_bk;
01515     joints--;
01516     reverse--;
01517     nb_joints++;
01518   }
01519   return ret_val;
01520 }

float KinematicChain::UpdateWithFullJacobian ( float *  angle_pos,
float *  angle_disp,
RigidTransfo visual_disp 
) [inherited]

Updates the kinematic chain rotation axes using kinetic updating algo, by comparing and joint angle displacement with the corresponding visual rotation.

See Micha Hersch's thesis for the formula

Parameters:
angle_pos the configuration of the kinematic chain (in rad)
angle_disp the angular displacement for all joints (in rad)
visual_disp the rotation resulting from the displacement
Returns:
the a measure of distance between the seen and the predicted end-effector rotation. If the sign is negative this distance is above the threshold and the chain was not updated (bad visual tracking assumed).

Definition at line 741 of file KinematicChain.cpp.

References Rotation::AddToAxis(), KinematicChain::GetFullJacobian(), KinematicChain::joints, and KinematicChain::SetAngles().

00742                                                                                  {
00743 
00744      CVector3_t delta_a[MAX_LINKS];
00745      CVector3_t v,vd,qd,qdiff,qds;
00746      CMatrix4_t m1,m2;
00747      CMatrix4_t rot_acc[MAX_LINKS];
00748      //  Rotation rot;
00749      float jac[MAX_LINKS*6];
00750      CQuat_t q1,q2,q3,q4,q7,q8;
00751      int i,j;
00752      float ret_val;//return value
00753      float sum_rot;
00754      SetAngles(angle_pos);
00755      GetFullJacobian(jac);
00756      v_clear(vd);//accumulator for v_dot
00757      v_clear(qd);//accumulator for q_dot
00758      for(i=0;i<nb_joints;i++){
00759         v_scale(jac+i*6+3,angle_disp[i],v);
00760         v_add(v,qd,qd);
00761      }
00762      if(v_length(qd)<epsilon){return 0;}//no movement
00763      rt->GetRotationParam(qds);//q_dot_star
00764      v_sub(qd,qds,qdiff);  
00765           ret_val = v_length(qdiff)/(v_length(qds));
00766      sum_rot=0;
00767     
00768 
00769      q_clear(q7); 
00770      q_clear(q8);
00771     
00772      for(j=0;j<nb_joints;j++){ //q8 is initialized with the gloabl rotation
00773          GetQuaternion(j,q2);
00774          q_multiply(q2,q8,q3);
00775          q_copy(q3,q8);
00776      }
00777 
00778      for(j=0;j<nb_joints;j++){
00779          m_clear(rot_acc[j]);
00780          if(j>0){
00781              GetQuaternion(j-1,q2);
00782              q_multiply(q7,q2,q3);
00783              q_copy(q3,q7);
00784          }
00785 
00786          GetInverseQuaternion(j,q2);
00787          q_multiply(q8,q2,q3);
00788          q_copy(q3,q8);
00789 
00790          q_clear(q1);         //initialization for i<j 
00791          q_copy(q7,q4);       //initialization for i<j 
00792          for(i=0;i<nb_joints;i++){ //k
00793              if(i<j){
00794                  QuaternionAxisDerivative(j,m1);
00795                  if(i>0){
00796                      GetQuaternion(i-1,q2);
00797                      q_multiply(q2,q1,q3);
00798                      q_copy(q3,q1);
00799                  }
00800                  GetInverseQuaternion(i,q2);
00801                  q_multiply(q4,q2,q3);
00802                  q_copy(q3,q4);
00803                
00804                  QuaternionAngleDerivative(i,q2);
00805                  q_multiply(q4,q2,q3);
00806                  q_multiply(q3,q1,q2); //before the matrix
00807                  q_copy(q8,q3);//after the matrix
00808              }
00809              if(i==j){   // i==j
00810                  QuaternionAxisAngleDerivative(j,m1);
00811                  q_copy(q8,q3);        // after the matrix
00812                  q_copy(q7,q2);        //before the matrix         
00813                  q_clear(q1);   //initialization for i>j 
00814                  q_copy(q8,q4); //initialization for i>j 
00815              }
00816              //i>j
00817              if(i>j){
00818                  QuaternionAxisDerivative(j,m1);
00819 
00820                  if(i>j+2){
00821                      GetQuaternion(i-1,q2);
00822                      q_multiply(q2,q1,q3);
00823                      q_copy(q3,q1);
00824                  }
00825                  
00826                  GetInverseQuaternion(i,q2);
00827                  q_multiply(q4,q2,q3);
00828                  q_copy(q3,q4);
00829 
00830                  QuaternionAngleDerivative(i,q3);
00831                  q_multiply(q4,q3,q2);
00832                  q_multiply(q2,q1,q3); //after the matrix
00833                  q_copy(q7,q2);// befire the matrix
00834              }
00835               
00836    //compute matrices            
00837              q_multiply(m1,q2,q4);
00838              q_copy(q4,m1);
00839              q_multiply(m1+4,q2,q4);
00840              q_copy(q4,m1+4);
00841              q_multiply(m1+8,q2,q4);
00842              q_copy(q4,m1+8);// last column of m1 is zero
00843                   
00844              q_multiply(q3,m1,m2);
00845              q_multiply(q3,m1+4,m2+4);
00846              q_multiply(q3,m1+8,m2+8);// last column of m1 is zero
00847              m_rescale(angle_disp[i],m2,m1);
00848              //accumulating sum_{i} d/daj(d/dthi(q)*thi
00849              // a index is horizontal, q index is vertical
00850              m_add(m1,rot_acc[j],rot_acc[j]);
00851          }
00852          v_transform_normal2(qdiff,rot_acc[j],delta_a[j]);
00853          v_scale(delta_a[j],-1,delta_a[j]); //gradient descent
00854          //cout<<"delta a "<<delta_a[j]<<endl;
00855          sum_rot += v_squ_length(delta_a[j]);
00856      }
00857      sum_rot = 0.5/sqrtf(sum_rot);
00858      for(j=0;j<nb_joints;j++){
00859          v_scale(delta_a[j],sum_rot,delta_a[j]);
00860          joints[j]->AddToAxis(delta_a[j]);
00861      }
00862      return ret_val;
00863 }

float KinematicChain::UpdateWithJacobian ( float *  angle_pos,
float *  angle_disp,
CVector3_t  visual_disp,
float  threshold,
CVector3_t  local_pos = NULL 
) [inherited]

Updates the kinematic chain rotation axes and translation vectors using kinetic updating algo, by comparing and joint angle displacement with the corresponding visual displacement.

See Micha Hersch's thesis for the formula

Parameters:
angle_pos the configuration of the kinematic chain (in rad)
angle_disp the angular displacement for all joints (in rad)
visual_disp the visual displacement of the end-effector
threshold the maximal distance between seen and predicted displacement of the end-effector for the update to take place. Used to discard outliers in the vision data
local_pos the position of the marker with respect to the end-effector
Returns:
the distance between the seen and the predicted end-effector displacement. If the sign is negative this distance is above the threshold and the chain was not updated (bad visual tracking assumed).

Definition at line 972 of file KinematicChain.cpp.

References Rotation::AngleDerivative(), Rotation::AxisDerivative(), KinematicChain::ForwardKinematicsStack(), KinematicChain::InitKinematicStack(), Rotation::Jacobian(), KinematicChain::joints, Rotation::NormDerivative(), KinematicChain::RotationStack(), Rotation::RotToMatrix(), KinematicChain::SetAngles(), Rotation::SetQuaternion(), and Rotation::Transform().

00973                                                                                {
00974   CVector3_t v1[MAX_LINKS],vv[MAX_LINKS], delta_rot[MAX_LINKS], delta_tr[MAX_LINKS];
00975   CQuat_t q1[MAX_LINKS];
00976   CMatrix3_t jstack[MAX_LINKS];
00977   CVector3_t v,v2,dpos;
00978   CQuat_t q2,q3,q4,q5,q6;
00979   int i,k;
00980   Rotation rot;
00981   CMatrix3_t m0,m1,m2,m3,m4,m6,m7;
00982   float f,sum_rot=0,sum_tr=0,ret_val;
00983   SetAngles(angle_pos);
00984   InitKinematicStack(local_pos,v2);
00985   ForwardKinematicsStack(v2,v1);
00986   RotationStack(q1);
00987   q_clear(q6);
00988 
00989   // computing jacobian
00990   v_clear(dpos);
00991   for(i=0;i<nb_joints;i++){
00992     rot.SetQuaternion(q1[i]);
00993     joints[i]->AngleDerivative(v1[i+1],v2);
00994     //   cout<<v1[i+1]<<endl;
00995     rot.Transform(v2,v);
00996     v_scale(v,angle_disp[i],v2);
00997     v_add(dpos,v2,dpos);
00998 }
00999 
01000 #ifdef COSINE_MIN
01001   float norm_prop = v_length(dpos);
01002   float norm_vis  = v_length(visual_disp);
01003   if(norm_vis> epsilon && norm_prop >epsilon){
01004     ret_val = v_dot(dpos,visual_disp)/(norm_prop*norm_vis);
01005   }
01006   else{
01007     cout<<norm_prop<<" - "<<norm_vis<<endl;
01008     return -2.0;
01009   }
01010 
01011 #ifdef WITH_CORRECTION
01012   // computing correction term for minimizing the angle (and not the dot product)
01013   CMatrix3_t m_corr;
01014   v_mult(dpos,dpos,m2);
01015   f= 1.f/(norm_prop*norm_prop);
01016   m_rescale(f,m2,m1);
01017   m_identity(m2);
01018   m_sub(m2,m1,m2);
01019   f=1.f/norm_prop;
01020   m_rescale(f,m2,m_corr);
01021 #else
01022   m_identity(m_corr);
01023 #endif
01024 
01025 
01026 #else
01027   CVector3_t diff;
01028    v_sub(visual_disp,dpos,diff);
01029    ret_val = v_length(diff);
01030   if(isnan(ret_val)){return -2.0;}
01031 #endif
01032 
01033 
01034   // pre-computing products (could be put in the loop below)
01035   for(k=0;k<nb_joints;k++){
01036     joints[k]->Jacobian(jstack[k]);
01037     v_transform_normal2(v1[k+1],jstack[k],vv[k]);
01038   //   cout<<k<<" vv "<<vv[k]<<"  "<< v1[k+1]<<endl;
01039 //     cout<<jstack[k]<<endl;
01040   }
01041   for(i=0;i<nb_joints;i++){
01042     m_clear(m4);//accumulator for dT/dli
01043     m_clear(m7);//accumulator for dT/dai
01044     q_clear(q2);
01045     q_clear(q3);
01046     joints[i]->AxisDerivative(v1[i+1],m0);
01047 
01048     for(k=0;k<i;k++){
01049       q_multiply(q2,q3,q4); //q2 = joints[k-1]
01050       q_copy(q4,q3);//outer rotations      
01051       joints[k]->GetQuaternion(q2);
01052       q_inv(q2,q5);// joints[k]
01053       q_multiply(q6,q5,q4);
01054       q_copy(q4,q6);//inner rotations
01055       rot.SetQuaternion(q6);
01056       rot.RotToMatrix(m6);//inner rotations
01057       rot.SetQuaternion(q3);
01058       rot.RotToMatrix(m3);//outer rotations 
01059 
01060       // translation update accumulator      
01061       m_identity(m2);
01062       joints[k]->NormDerivative(m6+0,m2+0);
01063       joints[k]->NormDerivative(m6+4,m2+4);
01064       joints[k]->NormDerivative(m6+8,m2+8);
01065  
01066       m3_multiply(m3,m2,m1);
01067       f =angle_disp[k]*0.5*joints[k]->GetAlpha();//converting to d/dtheta 
01068       m_rescale(f,m1,m2);
01069       m_add(m4,m2,m4);
01070 
01071       //rotation update accumulator
01072       m3_multiply(m6,m0,m2);
01073       m3_multiply(jstack[k],m2,m1);
01074       m3_multiply(m3,m1,m2);
01075       m_add(m7,m2,m7);
01076      }
01077 
01078                        
01079     // k=i
01080     q_multiply(q2,q3,q4); //q2 = joints[i-1]
01081     q_copy(q4,q3);      
01082     rot.SetQuaternion(q3);
01083     rot.RotToMatrix(m3);//outer rotation
01084 
01085     joints[i]->AngleAxisDerivative(v1[i+1],m2);
01086     m_rescale(angle_disp[i],m2,m1);
01087     m_add(m7,m1,m7);
01088 
01089     q_clear(q6);
01090     joints[i]->GetQuaternion(q2);
01091     for(k=i+1;k<nb_joints;k++){
01092       q_multiply(q2,q6,q4); //q2 = joint[k-1]
01093       q_copy(q4,q6);
01094       joints[k]->GetQuaternion(q2);
01095       rot.SetQuaternion(q6);
01096       rot.Transform(vv[k],v2);
01097       joints[i]->AxisDerivative(v2,m1);
01098       m3_multiply(m3,m1,m2);
01099       m_rescale(angle_disp[k],m2,m1);
01100       m_add(m7,m1,m7);
01101       if(isnan(m1[0])){
01102           cout<<m1<<endl;
01103           cout<<m3<<endl;
01104           cout<<vv[k]<<endl;
01105           cout<<i<<" "<<k<<endl;
01106           return -2.0;
01107       }
01108     }
01109 #ifdef COSINE_MIN
01110     m3_multiply(m_corr,m7,m1);
01111     v_transform_normal2(visual_disp,m1,delta_rot[i]);
01112     m3_multiply(m_corr,m4,m1);
01113     v_transform_normal2(visual_disp,m1,delta_tr[i]);
01114 #else
01115      v_transform_normal2(diff,m7,delta_rot[i]);
01116      v_transform_normal2(diff,m4,delta_tr[i]);
01117      //     cout<<delta_rot[i]<<endl;
01118 #endif
01119     sum_rot += v_squ_length(delta_rot[i]);  
01120     sum_tr += v_squ_length(delta_tr[i]);   
01121   }
01122 
01123 
01124 
01125   if(sum_rot>epsilon){//not necessary (already done in AddToAxis);
01126     sum_rot = 1/sqrt(sum_rot);
01127   }
01128   if(sum_tr>epsilon){
01129     sum_tr = 30/sqrt(sum_tr);//was 50
01130     //   sum_tr =0;
01131   }
01132   for(i=0;i<nb_joints;i++){
01133     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01134     v_scale(delta_rot[i],sum_rot,delta_rot[i]);// not necessary (already done in AddToAxis);
01135     
01136     joints[i]->Translation::Add(delta_tr[i]); 
01137     joints[i]->Rotation::AddToAxis(delta_rot[i]);
01138   }
01139   if(v_squ_length(v1[0])>300){
01140     //    ScaleChain(300);
01141   }
01142   return ret_val;
01143 }


Field Documentation

RigidTransfo** KinematicChain::joints [protected, inherited]
int* KinematicChain::reverse [protected, inherited]

if reverse[i] == -1 means that this rigid transfo is to be inverted.

Useful when loading a KinematicChain from an ArticulatedTree in ascending order. See Hersch et al, IJHR 2008 on http://infoscience.epfl.ch/record/117918 Fig 7 for more explanations.

Definition at line 69 of file KinematicChain.h.

Referenced by KinematicChain::Deserialize(), KinematicChain::GetFullJacobian(), KinematicChain::GetJacobian(), KinematicChain::GlobalRotation(), KinematicChain::RotationStack(), KinematicChain::Serialize(), KinematicChain::Transform(), KinematicChain::Update(), and KinematicChain::UpdateTouch().


The documentation for this class was generated from the following files:
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:27 2010 for Body_Schema_Learning by  doxygen 1.6.1