void SRS::SolveAim(float psi_angle, Matrix R1)
{
float h1[3], N[3], angle;
Matrix S0, S1;
// Get the final hand position
evalcircle(c, u, v, radius, psi_angle, h1);
// Rotate ee_r1 to h1
crossproduct(N, ee_r1, h1);
unitize(N);
angle = angle_between_vectors(ee_r1, h1, N);
rotation_axis_to_matrix(N, angle, S0);
// Now rotate a0 to a
float a[3], a0[3];
vecsub(a, (float*)ee, h1);
unitize(a);
hmatmult(S1,Ry,S0);
vecmult0(a0, (float*)axis, S1);
cpvector(N, h1);
unitize(N);
angle = angle_between_vectors(a0, a, N);
rotation_axis_to_matrix(N, angle, S1);
hmatmult(R1, S0, S1);
}
//
// p = Projection of u onto v
//
void project(float p[3], const float u[3], const float v[3])
{
float vnorm[3];
cpvector(vnorm, v);
unitize(vnorm);
vecscalarmult(p, vnorm, DOT(u,vnorm));
}
void SRS::SetAimGoal(const float goal[3],
const float ax[3],
float flex_angle)
{
float s[3];
cpvector(ee, goal);
cpvector(axis, ax);
get_translation(T, p_r1);
get_translation(S, s);
get_aim_circle_equation(goal,
axis, p_r1, s, proj_axis, flex_angle, c, u, v, radius);
rotation_principal_axis_to_matrix('y', flex_angle, Ry);
vecmult(ee_r1, (float*)s, Ry);
vecadd(ee_r1, ee_r1, (float*)p_r1);
}
//
// Constructor stores the T and S matrices and the
// lengths of the upper and lower links
//
void SRS::init(const Matrix T1, const Matrix T2, const float a[3], const float p[3])
{
cpmatrix(T, T1);
cpmatrix(S, T2);
cpvector(proj_axis, a);
cpvector(pos_axis, p);
float t[3];
get_translation(T, t);
upper_len = norm(t);
reciprocal_upper_len = 1.0f / upper_len;
get_translation(S, t);
lower_len = norm(t);
project_to_workspace = 1;
}
float get_circle_equation(const float ee[3],
const float axis[3],
const float pos_axis[3],
float upper_len,
float lower_len,
float c[3],
float u[3],
float v[3],
float n[3])
{
float wn = norm((float *)ee);
float radius;
cpvector(n, ee);
unitize(n);
// Use law of cosines to get angle between first spherical joint
// and revolute joint
float alpha;
if (!law_of_cosines(wn, upper_len, lower_len, alpha))
return 0;
// center of circle (origin is location of first S joint)
vecscalarmult(c, n, _cos(alpha) * upper_len);
radius = _sin(alpha) * upper_len;
float temp[3];
//
// A little kludgy. If the goal is behind the joint instead
// of in front of it, we reverse the angle measurement by
// inverting the normal vector
//
if (DOT(n,pos_axis) < 0.0)
vecscalarmult(n,n,-1.0);
vecscalarmult(temp, n, DOT(axis,n));
vecsub(u, (float *)axis, temp);
unitize(u);
crossproduct(v, n, u);
#if 0
printf("Circle equation\n");
printf("c = [%lf,%lf,%lf]\n", c[0], c[1], c[2]);
printf("u = [%lf,%lf,%lf]\n", u[0], u[1], u[2]);
printf("v = [%lf,%lf,%lf]\n", v[0], v[1], v[2]);
printf("n = [%lf,%lf,%lf]\n", n[0], n[1], n[2]);
printf("r = %lf\n", radius);
#endif
return radius;
}
//
// Return the angle between two vectors u,v about the axis n
//
float angle_between_vectors(float u[3], float v[3], float n[3])
{
#if 0
float temp[3];
float up[3];
float vp[3];
cpvector(up, u);
cpvector(vp, v);
unitize(up);
unitize(vp);
crossproduct(temp, up, vp);
float mag = DOT(temp,n);
// Vectors are parallel at 0 or 180
if (mag*mag < 1e-8)
{
if (DOT(up,vp) < 0)
return M_PI;
else
return 0;
}
int sign = (mag > 0) ? 1 : -1;
float t = DOT(up,vp);
if (t > 1.0)
t = 1.0;
else if (t < -1.0)
t = -1.0;
return sign*acos(t);
#else
float up[3];
float vp[3];
float uv[3];
project_plane(up, u, n);
project_plane(vp, v, n);
crossproduct(uv, up, vp);
return atan2(DOT(n, uv), DOT(up, vp));
#endif
}
//
// Evaluate a point on the circle given the swivel angle
//
void SRS::evaluate_circle(float angle, float p[3])
{
#if 1
evalcircle(c, u, v, radius, angle, p);
#else
// p = o + r*cos(f)*u + r*sin(f)*v
float temp[3];
cpvector(p, c);
vecscalarmult(temp, u, radius*cos(angle));
vecadd(p, p, temp);
vecscalarmult(temp, v, radius*sin(angle));
vecadd(p, p, temp);
#endif
}
inline void evalcircle(const float c[3],
const float u[3],
const float v[3],
float radius,
float angle,
float p[3])
{
// p = o + r*cos(f)*u + r*sin(f)*v
float temp[3];
cpvector(p, c);
vecscalarmult(temp, (float*)u, radius*_cos(angle));
vecadd(p, p, temp);
vecscalarmult(temp, (float*)v, radius*_sin(angle));
vecadd(p, p, temp);
}
int SRS::SetGoalPos(const float eee[3], const Matrix E, float &rangle)
{
Matrix Temp, RY;
float s[3];
// Find RY, and store the positions of the R jt and
// the ee in the R1 frame as p_r1 and ee_r1
get_translation(T, p_r1);
hmatmult(Temp,(float (*)[4]) E,S);
get_translation(Temp, s);
cpvector(ee,eee);
if (project_to_workspace)
scale_goal(p_r1,s,ee);
//
// Note instead of using the length of the lower limb
// we use the length of the lower limb extended by E
//
radius = get_circle_equation(ee,
proj_axis,
pos_axis,
upper_len,
norm(s), c, u, v, n);
if (!solve_R_angle(ee, s, p_r1, T, r_angle))
return 0;
rangle = r_angle;
// Find RY, and store the positions of the R jt and
// the ee in the R1 frame as p_r1 and ee_r1
rotation_principal_axis_to_matrix('y', r_angle, RY);
hmatmult(Temp, Temp, RY);
hmatmult(Temp, Temp, T);
get_translation(Temp, ee_r1);
return 1;
}
static void get_aim_circle_equation(const float g[3],
const float a[3],
const float ta[3],
const float tb[3],
const float proj_axis[3],
float theta4,
float center[3],
float u[3],
float v[3],
float &radius)
{
float L1 = DOT(ta,ta);
float L2 = DOT(tb,tb);
Matrix Ry, Ryt;
rotation_principal_axis_to_matrix('y', theta4, Ry);
invertrmatrix(Ryt, Ry);
// Compute distance of hand to shoulder
float t1[3], t2[3];
vecmult(t1, (float *) tb, Ry);
vecmult(t2, (float *) ta, Ryt);
float L3 = _sqrt(L1 + L2 + DOT(ta,t1) + DOT(tb,t2));
// Lengths of upper and lower arms
L1 = _sqrt(L1);
L2 = _sqrt(L2);
// Compute angle between a and shoulder-to-hand vector
// This is done assuming R1 = I since the angle does
// not depend on the shoulder joints
//
// h = Ry*tb + ta
// a = Ry*a
vecadd(t2, t1, (float *) ta);
unitize(t2);
vecmult(t1, (float *) a, Ry);
float alpha = acos(DOT(t1,t2));
//
// Compute the angles of the triangle s,h,g
//
float L4 = _sqrt(DOT(g,g));
float beta = M_PI - alpha;
float delta = asin(_sin(beta)*L3/L4);
if (delta < 0)
delta = - delta;
float gamma = M_PI - delta - beta;
float c_gamma = _cos(gamma);
float n[3];
cpvector(n, g);
unitize(n);
vecscalarmult(center, n, c_gamma*L3);
radius = _sqrt(1-c_gamma*c_gamma)*L3;
project_plane(u, (float *) proj_axis, n);
unitize(u);
crossproduct(v, n, u);
}
