/*
* Test 4: construct 1/(2^n-1)
*/
static void test4(void) {
rational_t r, aux, aux2;
uint32_t i;
printf("\nTest 4\n\n");
q_init(&r);
q_init(&aux);
q_init(&aux2);
q_set32(&aux, 2);
q_set_one(&aux2);
q_set_one(&r);
for (i=0; i<68; i++) {
q_inv(&r);
test_conversions(&r);
q_inv(&r);
q_mul(&r, &aux);
q_add(&r, &aux2);
}
q_set_minus_one(&r);
for (i=0; i<68; i++) {
q_inv(&r);
test_conversions(&r);
q_inv(&r);
q_mul(&r, &aux);
q_sub(&r, &aux2);
}
q_clear(&aux);
q_clear(&aux2);
q_clear(&r);
}
float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){
int i,j;
float cand,rdist=0,tdist=0,k=0.0001; //rotation vs translation weight
CVector3_t stack[MAX_LINKS];
CVector3_t tar,newrod,rod,diff,pos;
CQuat_t q1,q2,q3,q4,rot;
// converting data format
v_copy(position.GetArray(),pos);
q_complete(position.GetArray()+3,rot);
q_inv(rot,q1);
for(i=0;i<nb_joints;i++){
GetQuaternion(i,q2);
q_multiply(q2,q1,q3);
q_copy(q3,q1);
}
for(j=0;j<50;j++){
#ifdef WITH_LAST_LINK
last_link->GetTranslation(rod);
#else
v_clear(rod);
#endif
InverseKinematicsStack(pos,stack);
for(i=nb_joints-1;i>=0;i--){
GetInverseQuaternion(i,q2);
q_multiply(q2,q1,q3); //q1*inv(Ri)
if(IsInverted(i)==-1){
v_copy(stack[i],tar);
Translate(i,rod,newrod); //getting to joint
cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(-cand);// todo to check if it is -
Rotate(i,newrod,rod); // updating "rod"
v_sub(tar,rod,diff);
}
else{
Rotate(i,stack[i+1],tar); //rotating back
cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(cand);
Rotate(i,rod,newrod);
Translate(i,newrod,rod);
v_sub(tar,newrod,diff);
}
GetQuaternion(i,q2);
q_multiply(q3,q2,q1);
q_multiply(q2,q3,q4);
rdist = v_length(q4);//rotation distance, only the first 3 components
tdist = v_length(diff);//translation distance
// cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;}
}
q_multiply(rot,q1,q2);
q_inv(rot,q3);
q_multiply(q2,q3,q1);
}
return rdist/rot_tol + tdist;
}
void q_div ( Q a, Q x, Q y )
{
Q inv;
/* Doesn't seem to work. Will need to better understand inversion */
if ( q_is_zero(y) )
{
return;
}
q_inv(inv,y);
q_mul(a,x,inv);
}
void Scan::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& scan)
{
/* use the beam facing downwards to calculate the average height */
float sum = 0; /* sum of available distances detected */
int count = 0; /* number of available distances detected */
int scan_angle = 30; /* angle used for calculation in deg */
float intensities = 0;
float pitch = asin(2 * (q.q0 * q.q2 - q.q3 * q.q1));
/* only take the values within the set range */
for (int i = 90 - scan_angle/2 - RAD2DEG(pitch); i < 90 + scan_angle/2 - RAD2DEG(pitch); i++)
{
/* only take the values between the minimum and maximum value of the laser rangefinder */
if (scan->ranges[i]>scan->range_min && scan->ranges[i]<scan->range_max)
{
float angle = DEG2RAD(i);
/* convert the distances from the laser rangefinder into coordinates in the body frame */
Quaternion p_body;
p_body.q0 = 0;
p_body.q1 = offset[0] + scan->ranges[i] * cos(angle);
p_body.q2 = offset[1];
p_body.q3 = offset[2] - scan->ranges[i] * sin(angle);
/* convert the coordinates in body frame into ground frame */
Quaternion p_ground;
p_ground = q_mult(q_mult(q, p_body), q_inv(q));
/* the distance to the crop is z in the ground frame */
sum += p_ground.q3;
count++;
//intensities += scan->intensities[i];
}
}
/* publish the distance to the crop in Distance */
std_msgs::Float32 Distance;
Distance.data = sum / (count);
if (Distance.data == Distance.data)
{}
else
{
Distance.data = -6.0f;
}
CropDistance.publish(Distance);
ROS_INFO("\npitch:\t%f \ndist:\t%f \n",RAD2DEG(pitch), Distance.data);
}
inline void sortv(Iter first, Iter last, IterP p, Cmp cmp, Alloc alloc)
{
typedef typename std::iterator_traits<IterP>::value_type P;
typedef typename std::iterator_traits<IterP>::difference_type D;
D n = last - first;
std::vector<P, Alloc> q(n), q_inv(n);
for (D i = 0; i < n; ++i)
q_inv[i] = i;
std::sort(make_shadow_iter(first, q_inv.begin()),
make_shadow_iter(last, q_inv.end()),
shadow_cmp<Cmp>(cmp));
std::copy(q_inv, q_inv.end(), q.begin());
invert_permutation(q.begin(), q.end());
serialize_permutation(q.begin(), q.end(), q_inv.end(), p);
}
int test_inv ( void )
{
Q x[4];
Q inv;
void (*func[4])( Q x ) = {q_set_1, q_set_i, q_set_j, q_set_k};
char * label[4] = { "1", "i", "j", "k" };
char l[64];
int k;
printf("\n\n");
for ( k=0; k<4; ++k )
{
func[k](x[k]);
q_inv(inv,x[k]);
snprintf(l,64,"%s ",label[k]);
q_print_label(x[k],l);
snprintf(l,64,"Inv(%s)",label[k]);
q_print_label(inv,l);
printf("\n\n");
}
return 0;
}
