void Reaching::DecrementAngleWeights(float factor){
#ifdef DYN_WEIGHTS
v4_scale(base_weight_angle,factor,base_weight_angle);
#else
v4_scale(weight_angle,factor,weight_angle);
#endif
}
// works for homogeneous weights only. not clean
float Reaching::GetCosts(joint_vec_t& des_angle,cart_vec_t& des_pos){
joint_vec_t& v1,v2;
cart_vec_t& w1,w2;
float s1,s2;
v4_sub(des_angle,pos_angle,v1);
//m4_diag_v_multiply(weight_angle,v1,v2);
if (weight_angle[0] == 0){
s1 = v4_dot(v1,v1);
return s1;
}
else{
v4_scale(v1,1/weight_angle[0],v2);
s1 = v4_dot(v1,v2);
}
v_sub(des_cart,pos_cart,w1);
if (weight_cart[0] == 0){
s2 = v_dot(w1,w1);
return s2;
}
else{
v4_scale(w1,1/weight_cart[0],w2);
s2 = v_dot(w1,w2);
}
// m3_diag_v_multiply(weight_cart,w1,w2);
return (s1+s2)/(1/weight_cart[0]+1/weight_angle[0]);
}
void Reaching::IncrementAngleWeights(float factor){
float f = factor;
if (f<0) return;
#ifdef DYN_WEIGHTS
v4_scale(base_weight_angle,f,base_weight_angle);
#else
v4_scale(weight_angle,f,weight_angle);
#endif
}
// returns hoap angles in rad
void Reaching::RetrieveJointAngles(pHoap2_JointAngles angles)
{
joint_vec_t& v1,v2;
// Hoap2_JointAngles *angles;
// angles =(Hoap2_JointAngles *)an;
v4_copy(pos_angle,v1);
Rob2Hoap(v1);
v4_scale(v1,deg2rad,v2);
angles->R_SFE = v2[0];
angles->R_SAA = v2[1];
angles->R_SHR = v2[2];
angles->R_EB = v2[3];
}
/** Catmull-rom spline interpolation */
scalar_t spline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
{
vec4_t tmp;
scalar_t tsq = t * t;
static mat4_t crspline_mat = {
{-1, 3, -3, 1},
{2, -5, 4, -1},
{-1, 0, 1, 0},
{0, 2, 0, 0}
};
tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), crspline_mat), 0.5);
return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
}
/** b-spline approximation */
scalar_t bspline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
{
vec4_t tmp;
scalar_t tsq = t * t;
static mat4_t bspline_mat = {
{-1, 3, -3, 1},
{3, -6, 3, 0},
{-3, 0, 3, 0},
{1, 4, 1, 0}
};
tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), bspline_mat), 1.0 / 6.0);
return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
}
void Reaching::ReachingStep(float dt){
if(dt<EPSILON) return;//no point of doing anything
// cout<<"DT "<<dt<<endl;
joint_vec_t tmp1,tmp3; // temporary variables
cart_vec_t tmp13;
float dist=0;
// dt = dt/10;
//dist = UpdateLocalTarget();
if(dist>tol || isnan(dist)){
SetLocalTarget(target);
cout<<dist<<" "<<tol<<endl;
}
// vite in angle space and cart space
alpha*=dt;beta*=dt;
ViteAngle(tar_angle,pos_angle,v_angle,des_angle,des_v_angle);
ViteCart(target,pos_cart,v_cart,des_cart,des_v_cart);
alpha /= dt; beta/=dt;
// cout<<"vite done"<<endl;
#ifdef OBSTACLE_AVOIDANCE
// find the closest obstacle
float ro,point;
float gamma;
CMatrix4_t ijac;
v4_clear(des_a_angle_obs);
if(env){
for (i=1;i<3;i++){
for(int j=0;j<env->CountManipulableObjects();j++){
LinkDistanceToObject(i,env->GetObject(j),&ro,&point,tmp13);
// coutvec(tmp13);
IntermediateJacobian(i,point,pos_angle,ijac);
m3_4_t_v_multiply(ijac,tmp13,tmp1);
// eq. 18 of Khatib,icra'85
if(ro<=obstacle_rad){
gamma = nu *(1/ro -1/obstacle_rad)/(ro*ro); //(ro*ro)
//test
// gamma *= -v4_dot(tmp1,v_angle)/50;
// gamma = max(0.0,gamma);
}
else{
gamma =0;
}
v4_scale(tmp1,gamma,tmp1);
v4_add(des_a_angle_obs,tmp1,des_a_angle_obs);
}
}
v4_add(des_a_angle_obs,des_v_angle,des_v_angle);
v4_add(pos_angle,des_v_angle,des_angle);
}
#endif
body->SetAnglesInRange(des_angle);
des_v_angle = des_angle - pos_angle;
des_v_cart = des_cart - pos_cart;
if(pure_joint_ctl){
tmp1 = des_angle;
}
else{
UpdateWeights();
// coherence enforcement
ProjectVector(des_v_cart,des_v_angle,tmp3);
tmp1 = pos_angle+tmp3;
}
body->SetAnglesInRange(tmp1);
body->Angle2Cart(tmp1,tmp13);
v_angle = tmp1 - pos_angle;
v_cart = tmp13 - pos_cart;
pos_cart = tmp13;
pos_angle = tmp1;
//pos_angle.Print();
//pos_cart.Print();
}
