Exemplo n.º 1
0
std::vector<double> darwinop_kinematics_inverse_rarm(const double *dArm)
{
  std::vector<double> qArm(3,-999); // Init the 3 angles with value 0

    //printf("\n");
    double dx = dArm[0];
    double dy = -1*dArm[1] - shoulderOffsetY;
  double dz = dArm[2] - shoulderOffsetZ;
  double dc_sq = pow(dx,2)+pow(dy,2)+pow(dz,2);
  //printf("dx :%.3f, dy:%.3f, dz: %.3f\n",dx,dy,dz);

    // Get the elbow angle
  double tmp = (pow(upperArmLength,2)+pow(lowerArmLength,2)-dc_sq) / (2*upperArmLength*lowerArmLength);
  //double dc = sqrt( dc_sq );
  //printf("dc: %.3f, tmp: %.3f\n",dc,tmp);
  tmp = tmp>1 ? 1 : tmp;
  tmp = tmp<-1 ? -1 : tmp;
  double qbow = acos( tmp );
    //printf("tmp: %.3f, qbow: %.2f\n",tmp,qbow*180/PI);
    qArm[2] = -1*(PI - qbow);

    // Get to the correct y coordinate
    double hyp = upperArmLength+lowerArmLength*cos(PI-qbow);
    tmp = dy / hyp;
  tmp = tmp>1?1:tmp;
  tmp = tmp<-1?-1:tmp;
    double qroll = asin( tmp );
    //printf("dy: %.2f, hyp: %.2f\n",dy,hyp);
    //printf("qroll: %.2f\n",qroll*180/PI);
  qArm[1] = -1*qroll;

  /*
    tmp = (pow(hyp,2)-pow(dy,2));
    double x0 = sqrt( tmp<0?0:tmp );
    double z0 = lowerArmLength*sin(PI-qbow);
    double th0 = atan2(z0,x0);
    //printf("x0: %.2f, z0: %.2f\n",x0,z0);
    //printf("th0: %.2f, dth: %.2f\n",th0*180/PI,dth*180/PI);
  */

  qArm[0] = 0;
  Transform tbow = darwinop_kinematics_forward_rarm(&qArm[0]);
  double dz0 = tbow(2,3) - shoulderOffsetZ;
  double th0 = atan2( dz0, tbow(0,3) );
    //printf("qArm: %.2f, %.2f, %.2f\n",qArm[0],qArm[1],qArm[2]);
    //printf("\n");
    //printTransform(tbow);

  // Want to be at this theta
    double dth = atan2(dz,dx);
    qArm[0] = -1*(dth-th0);


  return qArm;
}
Exemplo n.º 2
0
static int forward_rarm(lua_State *L) {
  std::vector<double> q = lua_checkvector(L, 1);
  Transform t = darwinop_kinematics_forward_rarm(&q[0]);
  lua_pushtransform(L, t);
  return 1;
}