Reaching Class Reference

This class implements the multi-referential VITE reaching algorithm for an arbitary number of degrees of freedom. More...

#include <ReachingGen.h>

Public Member Functions

 Reaching ()
 constructor
virtual ~Reaching ()
 destructor
void SetBodySchema (BodySchema *b)
 sets the body schema
BodySchemaGetBodySchema ()
 retrieves the body schema
void SetTargetAngle (joint_vec_t &angles)
 sets the target angle
void GetTarget (cart_vec_t &pos)
 retrieves the target
void GetTargetAngle (joint_vec_t &joint)
 retrieves the target angle
int SetLocalTarget (cart_vec_t &new_target, int strict=0)
 sets a new target and computes the corresponding joint angle target
int TargetReached ()
 checks whether the target has been reached
void ReachingStep (float dt=1)
 Performs a reaching step, using the Grossberg's VITE algorithm on the cartesian and joint angle space.
void ViteCart (cart_vec_t &target, cart_vec_t &pos, cart_vec_t &speed, cart_vec_t &new_pos, cart_vec_t &new_speed)
 performs the vite dynamical system in cartesian space
void ViteAngle (joint_vec_t &target, joint_vec_t &pos, joint_vec_t &speed, joint_vec_t &new_pos, joint_vec_t &new_speed)
 performs the vite dynamical system in joint space
void ProjectVector (cart_vec_t &v3, joint_vec_t &v4, joint_vec_t &out)
 projects solves the constrained optimization problem, i.e., projects the desired positions on the manifold of consistent arm configuration and end-effector positions
void GetPosition (cart_vec_t &cart, joint_vec_t &joint)
 retrieves the joint angle and cartesian position
void GetAnglePosition (joint_vec_t &joint)
 retrieves the joint angle position
void MatchToBodySchema ()
 sets the position the the one of the body schema
void RandomTarget (int strict=1)
 sets a random target
void RandomTarget (cart_vec_t &tar, int strict=1)
 sets a random target
float UpdateLocalTarget (cart_vec_t *new_target=NULL)
 Updates the target in joint angle coordinates and in cartesian coordinates (if an argument is given) as a function of actual postion <pos_angle>.
void SetStill ()
 Stops reaching.
void PureJointControl ()
 Use a pure joint angle controller, no cartesian space controller.
void HybridControl ()
 Use a hybrid joint angle and cartesian space controller.
void Print ()
 Prints the position, and the target to cout.

Data Fields

BodySchemabody
cart_vec_t shoulder_abs_pos
float incr
cart_vec_t target
float tol
float zero_vel_angle
int pure_joint_ctl
cart_vec_t weight_cart
joint_vec_t weight_angle
cart_vec_t base_weight_cart
joint_vec_t base_weight_angle

Protected Member Functions

void UpdateWeights ()

Protected Attributes

float alpha
float beta
joint_vec_t v_angle
 angular velocity vector
cart_vec_t v_cart
 cartesian velocity vector
joint_vec_t des_angle
 desired angle position
cart_vec_t des_cart
 desired cartesian position
joint_vec_t pos_angle
 actual angle position
cart_vec_t pos_cart
 actual cartesian position
joint_vec_t tar_angle
 target angle position
joint_vec_t des_v_angle
 desired angular velocity vector
cart_vec_t des_v_cart
 desired cartesian velocity vector
ofstream * out_str
 for streaming into the output file

Detailed Description

This class implements the multi-referential VITE reaching algorithm for an arbitary number of degrees of freedom.

For being able to use it, you need to specify a BodySchema with which to perform the reaching. The body schema contains all the geometrical information about the limb you want to reach with

Definition at line 95 of file ReachingGen.h.


Member Function Documentation

void Reaching::GetAnglePosition ( joint_vec_t &  joint  )  [inline]

retrieves the joint angle position

Parameters:
joint where to put the joint angle position

Definition at line 313 of file ReachingGen.h.

References pos_angle.

00313 {joint = pos_angle;}

BodySchema * Reaching::GetBodySchema (  ) 

retrieves the body schema

Returns:
the BodySchema attached to this reaching

Definition at line 117 of file ReachingGen.cpp.

00117                                    {
00118     return body;
00119 }

void Reaching::GetPosition ( cart_vec_t &  cart,
joint_vec_t &  joint 
) [inline]

retrieves the joint angle and cartesian position

Parameters:
cart where to put the cartesian position
joint where to put the joint angle position

Definition at line 307 of file ReachingGen.h.

References pos_angle, and pos_cart.

00307 {cart=pos_cart;joint=pos_angle;}

void Reaching::GetTarget ( cart_vec_t &  pos  )  [inline]

retrieves the target

Parameters:
pos where to put the target

Definition at line 235 of file ReachingGen.h.

00235 {pos=target;};

void Reaching::GetTargetAngle ( joint_vec_t &  joint  )  [inline]

retrieves the target angle

Parameters:
joint where to put the target angle

Definition at line 241 of file ReachingGen.h.

References tar_angle.

Referenced by ReachingThread::run().

00241 {joint=tar_angle;}

void Reaching::ProjectVector ( cart_vec_t &  v3,
joint_vec_t &  v4,
joint_vec_t &  out 
)

projects solves the constrained optimization problem, i.e., projects the desired positions on the manifold of consistent arm configuration and end-effector positions

Parameters:
v3 desired end-effector position
v4 desired joint angle position
out where to put the resulting joint angle position

Definition at line 270 of file ReachingGen.cpp.

References pos_angle.

Referenced by ReachingStep().

00270                                                                                    {
00271     Matrix jac, 
00272         jact, m31,m32(cartesian_dim,cartesian_dim,false),
00273         wa(joint_angle_dim,joint_angle_dim,false); 
00274     joint_vec_t v41,v42;
00275     cart_vec_t v31,v32;
00276     body->Jacobian(pos_angle,jac); //J
00277     jact = jac.Transpose();
00278     wa.Diag(weight_angle);
00279     v31= vcart-jac*vangle;
00280     m31 = jac*wa*jact;
00281     m31 += m32.Diag(weight_cart);
00282     m32 = m31.Inverse();
00283     if(!Matrix::IsInverseOk()){cout<<"bad inversion"<<endl;}
00284     out = vangle+wa*jact*m32*v31;
00285 }

void Reaching::RandomTarget ( cart_vec_t &  tar,
int  strict = 1 
)

sets a random target

Parameters:
strict should be one if pointing is not allowed, 0 otherwise
tar the resulting target

Definition at line 127 of file ReachingGen.cpp.

References SetLocalTarget().

00127                                                      {
00128     joint_vec_t angles;
00129     body->GetRandomAngle(angles);
00130     body->Angle2Cart(angles,pos);
00131     body->SetRandomAngle(angles);
00132     SetLocalTarget(pos,strict);
00133 }

void Reaching::RandomTarget ( int  strict = 1  )  [inline]

sets a random target

Parameters:
strict should be one if pointing is not allowed, 0 otherwise

Definition at line 324 of file ReachingGen.h.

References RandomTarget().

Referenced by ReachingThread::init(), and RandomTarget().

00324 {cart_vec_t p;RandomTarget(p,strict);}

void Reaching::ReachingStep ( float  dt = 1  ) 

Performs a reaching step, using the Grossberg's VITE algorithm on the cartesian and joint angle space.

Unification is enforced by constraining the coherence of joint angles and cartesian positions (by use of lagrange multipliers)

Definition at line 195 of file ReachingGen.cpp.

References des_angle, des_cart, des_v_angle, des_v_cart, pos_angle, pos_cart, ProjectVector(), SetLocalTarget(), tar_angle, v_angle, v_cart, ViteAngle(), and ViteCart().

Referenced by ReachingThread::run().

00195                                    {
00196     if(dt<EPSILON) return;//no point of doing anything
00197     //  cout<<"DT "<<dt<<endl;
00198     joint_vec_t tmp1,tmp3; // temporary variables
00199     cart_vec_t tmp13;
00200     float dist=0;
00201   
00202     // dt = dt/10; 
00203     //dist = UpdateLocalTarget();
00204     if(dist>tol || isnan(dist)){
00205         SetLocalTarget(target);
00206         cout<<dist<<" "<<tol<<endl;
00207     }
00208     // vite in angle space and cart space
00209     alpha*=dt;beta*=dt;
00210     ViteAngle(tar_angle,pos_angle,v_angle,des_angle,des_v_angle);
00211     ViteCart(target,pos_cart,v_cart,des_cart,des_v_cart);
00212     alpha /= dt; beta/=dt;
00213 
00214     //  cout<<"vite done"<<endl; 
00215 #ifdef OBSTACLE_AVOIDANCE
00216     // find the closest obstacle
00217     float ro,point;
00218     float gamma;
00219     CMatrix4_t ijac;
00220     v4_clear(des_a_angle_obs);
00221     if(env){
00222         for (i=1;i<3;i++){
00223             for(int j=0;j<env->CountManipulableObjects();j++){
00224                 LinkDistanceToObject(i,env->GetObject(j),&ro,&point,tmp13);
00225                 //      coutvec(tmp13);
00226                 IntermediateJacobian(i,point,pos_angle,ijac);
00227                 m3_4_t_v_multiply(ijac,tmp13,tmp1);
00228                 // eq. 18 of Khatib,icra'85
00229                 if(ro<=obstacle_rad){
00230                     gamma = nu *(1/ro -1/obstacle_rad)/(ro*ro); //(ro*ro) 
00231                     //test
00232                     //   gamma *= -v4_dot(tmp1,v_angle)/50;
00233                     //   gamma = max(0.0,gamma);
00234                 }
00235                 else{
00236                     gamma =0;
00237                 }
00238                 v4_scale(tmp1,gamma,tmp1);
00239                 v4_add(des_a_angle_obs,tmp1,des_a_angle_obs);
00240             }
00241         }
00242         v4_add(des_a_angle_obs,des_v_angle,des_v_angle);
00243         v4_add(pos_angle,des_v_angle,des_angle);
00244     }
00245 #endif
00246  
00247     body->SetAnglesInRange(des_angle);
00248     des_v_angle = des_angle - pos_angle;
00249     des_v_cart = des_cart - pos_cart;
00250     if(pure_joint_ctl){
00251         tmp1 = des_angle;
00252     }
00253     else{
00254         UpdateWeights();
00255         // coherence enforcement
00256         ProjectVector(des_v_cart,des_v_angle,tmp3);
00257         tmp1 = pos_angle+tmp3;
00258     }
00259     body->SetAnglesInRange(tmp1);
00260     body->Angle2Cart(tmp1,tmp13);
00261     v_angle = tmp1 - pos_angle;
00262     v_cart = tmp13 - pos_cart;
00263     pos_cart = tmp13;
00264     pos_angle = tmp1;
00265     pos_angle.Print();
00266     pos_cart.Print();
00267 }

int Reaching::SetLocalTarget ( cart_vec_t &  new_target,
int  strict = 0 
)

sets a new target and computes the corresponding joint angle target

setting the target relatively to shoulder position

Parameters:
new_target target in local referential
strict if 0, pointing if target out if range, else no pointing
Returns:
1 if target is reachable
0 otherwise
Parameters:
new_target target in local referential
Returns:
returns 1 if target is reachable
-1 if target is pointable, 0 otherwise

Definition at line 153 of file ReachingGen.cpp.

References pos_angle, and tar_angle.

Referenced by RandomTarget(), ReachingStep(), and UpdateLocalTarget().

00154 {
00155     int res;
00156     joint_vec_t tangle;
00157     body->SetPosition(pos_angle);// trial 
00158     res = body->InverseKinematics(new_target, tangle);
00159     if(res){
00160         target = new_target;
00161 #ifndef SILENT 
00162         cout << "reachable target ";new_target.Print();
00163 #endif
00164     }
00165     else{
00166 #ifndef SILENT 
00167         cout << "unreachable target ";new_target.Print();
00168 #endif
00169         if(strict){
00170             // ignoring unreachable target 
00171             //  tar_angle = pos_angle;
00172             //  target = pos_cart;
00173             return 0;
00174         } 
00175         else{
00176             body->Angle2Cart(target);
00177         }
00178     }
00179     tar_angle = tangle;
00180     return res;
00181 }

void Reaching::SetTargetAngle ( joint_vec_t &  angles  ) 

sets the target angle

Parameters:
angles desired target angle

Definition at line 141 of file ReachingGen.cpp.

References tar_angle.

00141                                                 {
00142     body->SetAnglesInRange(angles);
00143     tar_angle = angles;
00144     body->Angle2Cart(tar_angle,target);
00145 }

int Reaching::TargetReached (  )  [inline]

checks whether the target has been reached

Returns:
1 if the target has been reached (position and velocities are within some boundaries given by tol and zero_vel_angle), 0 otherwise

Definition at line 258 of file ReachingGen.h.

References pos_cart, and v_angle.

00258 {return (target-pos_cart).Norm2() < 2*tol*tol && v_angle.Norm2()<zero_vel_angle*zero_vel_angle;}

float Reaching::UpdateLocalTarget ( cart_vec_t *  new_target = NULL  ) 

Updates the target in joint angle coordinates and in cartesian coordinates (if an argument is given) as a function of actual postion <pos_angle>.

Parameters:
new_target the new target (if changed), assumed to be very closed from the previous one.
Returns:
the discrepancy (distance) between joint angle and cartesian targets, a negative number if the new target is not close enough from the old one.

Definition at line 348 of file ReachingGen.cpp.

References pos_angle, SetLocalTarget(), and tar_angle.

00348                                                        {
00349     cart_vec_t diff,v1,v2;
00350     joint_vec_t v3,v4,v5;
00351     Matrix m1,m2;
00352     Matrix jac,jacT,m3,m4;
00353     float f;
00354     if(new_target){
00355         diff = *new_target-target;
00356         if(diff.Norm2()>tol*tol){
00357             return SetLocalTarget(*new_target);
00358             //      return -1.0f;
00359         }
00360         else{
00361             //updating cartesian target
00362             target=*new_target;
00363         }
00364     }
00365     body->Angle2Cart(tar_angle,v1);
00366     v2=target-v1;
00367     body->Jacobian(tar_angle,jac);
00368      jac.Inverse(m3);//todo : find the weighted version
00369 
00370     if(!Matrix::IsInverseOk()){cout<<"bad pseudo-inverse in Reaching::UpdateLocalTarget"<<endl;}
00371     v3 = m3*v2*0.1;
00372     m4 = m3*jac;
00373     m3.Resize(tar_angle.Size(),tar_angle.Size());
00374     m3.Identity();
00375     v5=(m4-m3)*(tar_angle-pos_angle);
00376     if((f=v5.Norm2())>0.01*0.01){
00377         v5*= 0.001/sqrtf(f);
00378     }
00379     v4 = v5+v3; 
00380     v3 = tar_angle+v4;
00381  
00382     if(body->AnglesInRange(v3)){
00383         tar_angle=v3;
00384     }
00385     //checking precision
00386     body->Angle2Cart(tar_angle,v1);
00387     return body->CartesianDistance(target,v1); 
00388 }

void Reaching::ViteAngle ( joint_vec_t &  target,
joint_vec_t &  pos,
joint_vec_t &  speed,
joint_vec_t &  new_pos,
joint_vec_t &  new_speed 
)

performs the vite dynamical system in joint space

performs the vite dynamical system in 4d

Parameters:
target the target
pos the position at time t
speed the speed at time t
new_pos the position at time t+1 (output of the method)
new_speed the speed at time t+1 (output of the method
target the target
pos the position at time t
speed the speed at time t
new_pos the position at time t+1 (output of the method)
new_speed the speed at time t+1 (output of the method

Definition at line 317 of file ReachingGen.cpp.

Referenced by ReachingStep().

00318                                                                        {
00319         //vite in joint space
00320         // v update
00321         new_speed  = speed + ((target-pos)*beta - speed)*alpha;
00322         new_pos = pos+new_speed;
00323 }

void Reaching::ViteCart ( cart_vec_t &  target,
cart_vec_t &  pos,
cart_vec_t &  speed,
cart_vec_t &  new_pos,
cart_vec_t &  new_speed 
)

performs the vite dynamical system in cartesian space

performs the vite dynamical system in 3d

Parameters:
target the target
pos the position at time t
speed the speed at time t
new_pos the position at time t+1 (output of the method)
new_speed the speed at time t+1 (output of the method
target the target
pos the position at time t
speed the speed at time t
new_pos the position at time t+1 (output of the method)
new_speed the speed at time t+1 (output of the method

Definition at line 298 of file ReachingGen.cpp.

Referenced by ReachingStep().

00299                                                                     {
00300     //  cart_vec_t tmp1,tmp2;
00301         //vite in cart space
00302         // v update
00303     new_speed  = speed + ((target-pos)*beta - speed)*alpha;
00304     new_pos = pos+new_speed;
00305     //  cout<<"new cart speed "<<speed<<endl;
00306 }


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