int
KinematicSolver::forward(btTransform& pose) const {
TRACER_ENTER_SCOPE("KinematicSolver::forward()");
if (false && forwardKinTimestamp_ == arm_->timestamp()) {
pose = forwardKinCached_;
return 0;
}
Arm::IdType armId_new = arm_->id();
int armId = armIdFromSerial(arm_->id());
pose = actual_world_to_ik_world(armId)
* Tw2b
* Zs(THS_TO_IK(armId,arm_->getJointById(Joint::IdType::SHOULDER_)->position()))
* Xu
* Ze(THE_TO_IK(armId,arm_->getJointById(Joint::IdType::ELBOW_)->position()))
* Xf
* Zr(THR_TO_IK(armId,arm_->getJointById(Joint::IdType::ROTATION_)->position()))
* Zi(D_TO_IK(armId,arm_->getJointById(Joint::IdType::INSERTION_)->position()))
* Xip
* Zp(THP_TO_IK(armId,arm_->getJointById(Joint::IdType::WRIST_)->position()))
* Xpy
* Zy(THY_TO_IK_FROM_FINGERS(armId,arm_->getJointById(Joint::IdType::FINGER1_)->position(),arm_->getJointById(Joint::IdType::FINGER2_)->position()))
* Tg;
//int grasp = MECH_GRASP_FROM_MECH_FINGERS(armId,arm_->getJointById(Joint::Type::GRIPPER1_)->position(),arm_->getJointById(Joint::Type::GRIPPER2_)->position());
const_cast<KinematicSolver*>(this)->forwardKinCached_ = pose;
const_cast<KinematicSolver*>(this)->forwardKinTimestamp_ = arm_->timestamp();
return 0;
}
const M2_arrayint ARingGF::findMinimalPolynomial(UTT charac, UTT dim)
{
//ARingGF tmp(charac,dim);
//return tmp.getModPolynomialCoeffs();
FieldType Zp(charac,1);
// typedef CyclotomicTable< GFqDom<TT>, Dense > PolDom;
// PolDom Pdom( Zp, e );
typedef Givaro::Poly1FactorDom< FieldType::Self_t, Givaro::Dense > PolDom;
PolDom Pdom( Zp );
PolDom::Element F, G, H;
// F is irreducible of degree e over Zp
// G is a primitive polynomial for F
// Pdom.random_prim_root(F,G, Degree(e));
// F is an irreducible factor of the
// (p^e-1) th cyclotomic polynomial
// G is a primitive polynomial for F : X
// Pdom.getcyclo(F);
// Pdom.init(G, Degree(1), Zp.one);
// F is irreducible of degree e over Zp
// with X as a primitive polynomial
#ifndef GIVARO_RANDOM_IRREDUCTIBLE_PRIMITIVE_ROOT
Pdom.ixe_irreducible(F, Givaro::Degree((long)dim));
// Pdom.init(G, Degree(1), Zp.one);
// Pdom.assign(G, Degree(1), Zp.one);
Pdom.init(G, Givaro::Degree(1));
#else
Pdom.random_irreducible(F, Givaro::Degree((long)dim));
Pdom.give_random_prim_root(G,F);
#endif
Pdom.assign(H, G);
typedef Givaro::Poly1PadicDom< FieldType, Givaro::Dense > PadicDom;
PadicDom PAD(Pdom);
UTT generator, irreducible;
PAD.eval(generator, H);
PAD.eval(irreducible, F);
return (representationToM2Array(irreducible, dim+1 ,charac) );
}
/*
* OpenGL (GLUT) calls this routine to display the scene
*/
void display()
{
const double len=1.5; // Length of axes
// Erase the window and the depth buffer
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
// Undo previous transformations
glLoadIdentity();
// Set view angle
glRotatef(ph,1,0,0);
glRotatef(th,0,1,0);
// Enable Z-buffering
if (mode==2 || mode==3)
glEnable(GL_DEPTH_TEST);
else
glDisable(GL_DEPTH_TEST);
// Enable face culling
if (mode!=2)
glEnable(GL_CULL_FACE);
else
glDisable(GL_CULL_FACE);
// Draw a single cube
if (n==1)
cube(0,0,0 , 1,1,1 , 0);
// Multiple cubes - fixed order
else if (mode != 1)
{
int k;
for (k=0;k<n;k++)
cube(Cube[k].x,Cube[k].y,Cube[k].z , Cube[k].dx,Cube[k].dy,Cube[k].dz , Cube[k].th);
}
// Multiple cubes - Z-sorted
else
{
int i,k;
cube_t* cubep[Nk];
// Calculate projection Z component
for (k=0;k<n;k++)
Cube[k].Zp = Zp(Cube[k].x,Cube[k].y,Cube[k].z);
// Initialize Z pointer array
for (k=0;k<n;k++)
cubep[k] = Cube+k;
// Sort cubes on Z ascending using bubble sort
for (k=0;k<n-1;k++)
for (i=k+1;i<n;i++)
if (cubep[k]->Zp > cubep[i]->Zp)
{
cube_t* t = cubep[k];
cubep[k] = cubep[i];
cubep[i] = t;
}
// Draw cubes
for (k=0;k<n;k++)
cube(cubep[k]->x,cubep[k]->y,cubep[k]->z , cubep[k]->dx,cubep[k]->dy,cubep[k]->dz , cubep[k]->th);
// Print sequence
glColor3f(1,1,1);
glWindowPos2i(5,25);
Print("Z order");
for (k=0;k<n;k++)
Print(" %d=%.3f",cubep[k]->id,cubep[k]->Zp);
}
// White
glColor3f(1,1,1);
// Draw axes
if (axes)
{
glBegin(GL_LINES);
glVertex3d(0.0,0.0,0.0);
glVertex3d(len,0.0,0.0);
glVertex3d(0.0,0.0,0.0);
glVertex3d(0.0,len,0.0);
glVertex3d(0.0,0.0,0.0);
glVertex3d(0.0,0.0,len);
glEnd();
// Label axes
glRasterPos3d(len,0.0,0.0);
Print("X");
glRasterPos3d(0.0,len,0.0);
Print("Y");
glRasterPos3d(0.0,0.0,len);
Print("Z");
}
// Five pixels from the lower left corner of the window
glWindowPos2i(5,5);
// Print the text string
Print("Angle=%d,%d Hidden=%s",th,ph,text[mode]);
if (rev) Print(" Reverse");
// Render the scene
glFlush();
// Make the rendered scene visible
glutSwapBuffers();
}
InverseKinematicsReportPtr
KinematicSolver::internalInverseSoln(const btTransform& pose, Arm* arm,const InverseKinematicsOptions& options) const {
TRACER_ENTER_SCOPE("KinematicSolver::internalInverseSoln(arm@%p)",arm);
InverseKinematicsReportPtr report(new InverseKinematicsReport());
Arm::IdType armId_new = arm_->id();
int armId = armIdFromSerial(arm_->id());
// desired tip position
// btVector3 currentPoint = btVector3(mech->pos.x,mech->pos.y,mech->pos.z) / MICRON_PER_M;
// btVector3 actualPoint = btVector3(pos_d->x,pos_d->y,pos_d->z) / MICRON_PER_M;
// btMatrix3x3 actualOrientation = toBt(ori_d->R);
// btTransform actualPose(actualOrientation,actualPoint);
// btTransform actualPose_fk = pose;
// tb_angles currentPoseAngles = tb_angles(mech->ori.R);
// tb_angles actualPoseAngles = tb_angles(ori_d->R);
//float grasp = GRASP_TO_IK(armId,mech->ori_d.grasp);
float grasp = arm->getJointById(Joint::IdType::GRASP_)->position();
// if (print) {
// log_msg("j s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",
// arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position(),
// arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG,
// fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() - arm->getJointById(Joint::Type::GRIPPER2_)->position()) / 2 RAD2DEG,
// (arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG,arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG);
// log_msg("v s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",
// arm->getJointById(Joint::Type::SHOULDER].jvel RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW].jvel RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT].jvel RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION_].jvel,
// arm->getJointById(Joint::Type::WRIST].jvel RAD2DEG,
// (arm->getJointById(Joint::Type::GRASP1].jvel - arm->getJointById(Joint::Type::GRASP2].jvel) / 2 RAD2DEG,
// arm->getJointById(Joint::Type::GRASP1].jvel + arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG,
// arm->getJointById(Joint::Type::GRASP1].jvel RAD2DEG,arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG);
// log_msg("t s % 1.3f e % 1.3f r % 1.3f i % 1.3f p % 1.3f g1 % 1.3f g2 % 1.3f",arm->getJointById(Joint::Type::SHOULDER].tau_d,
// arm->getJointById(Joint::Type::ELBOW].tau_d,arm->getJointById(Joint::Type::TOOL_ROT].tau_d,arm->getJointById(Joint::Type::INSERTION_].tau_d,arm->getJointById(Joint::Type::WRIST].tau_d,
// arm->getJointById(Joint::Type::GRASP1].tau_d,arm->getJointById(Joint::Type::GRASP2].tau_d);
// log_msg("d s %d e %d r %d i %d p %d g1 %d g2 %d",tToDACVal(&(arm->getJointById(Joint::Type::SHOULDER])),
// tToDACVal(&(arm->getJointById(Joint::Type::ELBOW])),tToDACVal(&(arm->getJointById(Joint::Type::TOOL_ROT])),tToDACVal(&(arm->getJointById(Joint::Type::INSERTION_])),tToDACVal(&(arm->getJointById(Joint::Type::WRIST])),
// tToDACVal(&(arm->getJointById(Joint::Type::GRASP1])),tToDACVal(&(arm->getJointById(Joint::Type::GRASP2])));
// log_msg("cp (% 1.3f,% 1.3f,% 1.3f\typr (% 1.3f,% 1.3f,% 1.3f))",
// currentPoint.x(),currentPoint.y(),currentPoint.z(),
// currentPoseAngles.yaw_deg,currentPoseAngles.pitch_deg,currentPoseAngles.roll_deg);
// log_msg("pt (% 1.3f,% 1.3f,% 1.3f)\typr (% 1.3f,% 1.3f,% 1.3f)\tg % 1.3f",
// actualPoint.x(),actualPoint.y(),actualPoint.z(),
// actualPoseAngles.yaw_deg,actualPoseAngles.pitch_deg,actualPoseAngles.roll_deg,
// grasp);
//
// btVector3 point = actualPose_fk.getOrigin();
// tb_angles angles = tb_angles(actualPose_fk.getBasis());
// log_msg("fp (% 1.3f,% 1.3f,% 1.3f)\typr (% 2.1f,% 2.1f,% 2.1f)\tg % 1.3f",
// point.x(),point.y(),point.z(),
// angles.yaw_deg,angles.pitch_deg,angles.roll_deg,
// grasp);
//
// }
/*
* Actual pose is in the actual world frame, so we have <actual_world to gripper>
* The ik world frame is the base frame.
* Therefore, we need <base to actual_world> to get <base to gripper>.
* <base to actual_world> is inverse of <actual_world to base>
*
* Since the ik is based on the yaw frame (to which the gripper is fixed), we
* take the pose of the yaw frame, not the gripper frame
*/
btTransform ik_pose = ik_world_to_actual_world(armId) * pose * Tg.inverse();
btMatrix3x3 ik_orientation = ik_pose.getBasis();
btVector3 ik_point = ik_pose.getOrigin();
tb_angles ikPoseAngles = tb_angles(ik_pose);
// if (print) {
// log_msg("ik (%0.4f,%0.4f,%0.4f)\typr (%0.4f,%0.4f,%0.4f)",
// ik_point.x(),ik_point.y(),ik_point.z(),
// ikPoseAngles.yaw_deg,ikPoseAngles.pitch_deg,ikPoseAngles.roll_deg);
// }
float ths_offset, thr_offset, thp_offset;
if (arm->isGold()) {
ths_offset = SHOULDER_OFFSET_GOLD;
thr_offset = TOOL_ROT_OFFSET_GOLD;
thp_offset = WRIST_OFFSET_GOLD;
} else {
ths_offset = SHOULDER_OFFSET_GREEN;
thr_offset = TOOL_ROT_OFFSET_GREEN;
thp_offset = WRIST_OFFSET_GREEN;
}
const float th12 = THETA_12;
const float th23 = THETA_23;
const float ks12 = sin(th12);
const float kc12 = cos(th12);
const float ks23 = sin(th23);
const float kc23 = cos(th23);
const float dw = DW;
btTransform Tworld_to_gripper = ik_pose;
btTransform Tgripper_to_world = ik_pose.inverse();
// if (print) {
// log_msg("Tw2g: %d",PRINT_EVERY);
// for (int i=0;i<3;i++) {
// log_msg(" %0.4f\t%0.4f\t%0.4f\t%0.4f",ik_pose.getBasis()[i][0],ik_pose.getBasis()[i][1],ik_pose.getBasis()[i][2],ik_pose.getOrigin()[i]);
// }
// }
btVector3 origin_in_gripper_frame = Tgripper_to_world.getOrigin();
float px = origin_in_gripper_frame.x();
float py = origin_in_gripper_frame.y();
float pz = origin_in_gripper_frame.z();
float thy = atan2f(py,-px);
float thp;
if (fabs(thy) < 0.001) {
thp = atan2f(-pz, -px/cos(thy) - dw);
// if (print) { log_msg("zero thy: %0.4f, (%0.4f, %0.4f, %0.4f)",thp,px,py,pz); }
} else {
thp = atan2f(-pz, py/sin(thy) - dw);
}
float d = -pz / sin(thp);
float d_act, thp_act, thy_act, g1_act, g2_act;
d_act = D_FROM_IK(armId,d);
thp_act = THP_FROM_IK(armId,thp);
thy_act = THY_FROM_IK(armId,thy,grasp);
g1_act = FINGER1_FROM_IK(armId,thy,grasp);
g2_act = FINGER2_FROM_IK(armId,thy,grasp);
//check angles
int validity1[4];
bool valid1 = checkJointLimits1(d_act,thp_act,g1_act,g2_act,validity1);
if (!valid1) {
// if (_curr_rl == 3 && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d invalid --1-- d [%d] % 2.4f \tp [%d] % 3.1f\ty [%d %d] % 3.1f\n",
// armId,
// validity1[0], d_act,
// validity1[1], thp_act RAD2DEG,
// validity1[2],validity1[3], thy_act RAD2DEG);
// }
return report;
}
//set joints
//setJointsWithLimits1(mech,d_act,thp_act,thy_act,grasp);
btVector3 z_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(0,0,1));
btVector3 x_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(1,0,0));
float zx = z_roll_in_world.x();
float zy = z_roll_in_world.y();
float zz = z_roll_in_world.z();
float xx = x_roll_in_world.x();
float xy = x_roll_in_world.y();
float xz = x_roll_in_world.z();
float cthe = (zy + kc12*kc23) / (ks12*ks23);
float the_1 = acos(cthe);
float the_2 = -acos(cthe);
float the_opt[2];
the_opt[0] = the_1;
the_opt[1] = the_2;
float ths_opt[2];
float thr_opt[2];
bool opts_valid[2];
float validity2[2][4];
float ths_act[2];
float the_act[2];
float thr_act[2];
for (int i=0;i<2;i++) {
float sthe_tmp = sin(the_opt[i]);
float C1 = ks12*kc23 + kc12*ks23*cthe;
float C2 = ks23 * sthe_tmp;
float C3 = C2 + C1*C1 / C2;
ths_opt[i] = atan2(
-sgn(C3)*(zx - C1 * zz / C2),
sgn(C3)*(zz + C1 * zx / C2));
float sths_tmp = sin(ths_opt[i]);
float cths_tmp = cos(ths_opt[i]);
float C4 = ks12 * sin(the_opt[i]);
float C5 = kc12 * ks23 + ks12 * kc23 * cos(the_opt[i]);
float C6 = kc23*(sthe_tmp * sths_tmp - kc12*cthe*cths_tmp) + cths_tmp*ks12*ks23;
float C7 = cthe*sths_tmp + kc12*cths_tmp*sthe_tmp;
thr_opt[i] = atan2(
(xx - C7 * xy / C4) / (C6 + C7*C5/C4),
(xx + C6 * xy / C5) / (-C6*C4/C5 - C7));
ths_act[i] = THS_FROM_IK(armId,ths_opt[i]);
the_act[i] = THE_FROM_IK(armId,the_opt[i]);
thr_act[i] = THR_FROM_IK(armId,thr_opt[i]);
// if (print) {
// log_msg("j s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",
// arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position(),
// arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG,
// THY_MECH_FROM_FINGERS(armIdFromMechType(mech->type),arm->getJointById(Joint::Type::GRIPPER1_)->position(), arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,//fix_angle(arm->getJointById(Joint::Type::GRIPPER2_)->position() - arm->getJointById(Joint::Type::GRIPPER1_)->position(),0) / 2 RAD2DEG,
// mech->ori.grasp * 1000. RAD2DEG,
// fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position(),0) RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG, arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG);
// log_msg("%d s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",i,
// ths_act[i] RAD2DEG,
// the_act[i] RAD2DEG,
// thr_act[i] RAD2DEG,
// d_act,
// thp_act RAD2DEG,
// thy_act RAD2DEG,
// grasp RAD2DEG,
// g1_act RAD2DEG,
// g2_act RAD2DEG);
//
// if (ths_act != ths_act) {
// log_msg("C1 %0.4f\tC2 %0.4f\tC3 %0.4f\tC4 %0.4f\tC5 %0.4f\tC6 %0.4f\tC7 %0.4f\t",C1,C2,C3,C4,C5,C6,C7);
// log_msg("ks23 %0.4f\tsthe_tmp %0.4f",ks23 , sthe_tmp);
// log_msg("cthe %0.4f\tzy %0.4f\tkc12 %0.4f\tkc23 %0.4f\tks12 %0.4f\tks23 %0.4f",cthe,zy,kc12,kc23,ks12,ks23);
// }
// }
bool valid2 = checkJointLimits2(ths_act[i],the_act[i],thr_act[i],validity2[i]);
opts_valid[i] = valid2;
if (valid2) {
float ths_diff, the_diff, d_diff, thr_diff, thp_diff, thg1_diff, thg2_diff;
//set joints
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[i],the_act[i],thr_act[i]);
ths_diff = arm->getJointById(Joint::IdType::SHOULDER_)->position() - ths_act[i];
the_diff = arm->getJointById(Joint::IdType::ELBOW_)->position() - the_act[i];
d_diff = arm->getJointById(Joint::IdType::INSERTION_)->position() - d_act;
thr_diff = arm->getJointById(Joint::IdType::ROTATION_)->position() - thr_act[i];
thp_diff = arm->getJointById(Joint::IdType::WRIST_)->position() - thp_act;
thg1_diff = arm->getJointById(Joint::IdType::FINGER1_)->position() - g1_act;
thg2_diff = arm->getJointById(Joint::IdType::FINGER2_)->position() - g2_act;
/*
ths_diff = arm->getJointById(Joint::Type::SHOULDER_)->position()_d - ths_act;
the_diff = arm->getJointById(Joint::Type::ELBOW_)->position()_d - the_act;
d_diff = arm->getJointById(Joint::Type::INSERTION__)->position()_d - d_act;
thr_diff = arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d - thr_act;
thp_diff = arm->getJointById(Joint::Type::WRIST_)->position()_d - thp_act;
thg1_diff = arm->getJointById(Joint::Type::GRIPPER1_)->position()_d - g1_act;
thg2_diff = arm->getJointById(Joint::Type::GRIPPER2_)->position()_d - g2_act;
*/
// if (print) {
// log_msg("%d s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",i,
// arm->getJointById(Joint::Type::SHOULDER_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position()_d,
// arm->getJointById(Joint::Type::WRIST_)->position()_d RAD2DEG,
// grasp RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER2_)->position()_d RAD2DEG);
// log_msg("diff:");
// log_msg("R s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f",
// ths_diff,the_diff,thr_diff,d_diff,thp_diff,thg1_diff,thg2_diff);
// log_msg("D s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f",
// ths_diff*180/M_PI,the_diff*180/M_PI,thr_diff*180/M_PI,d_diff,
// thp_diff*180/M_PI,thg1_diff*180/M_PI,thg2_diff*180/M_PI);
//
// }
//
// if (i==1 && _curr_rl == 3) {
// //printf("ik ok! %d\n",_ik_counter);
// }
}
}
if (opts_valid[0]) {
report->success_ = true;
return report;
} else if (opts_valid[1]) {
// bool ENABLE_PARTIAL_INVALID_IK_PRINTING = false;
// if (ENABLE_PARTIAL_INVALID_IK_PRINTING && (_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d ok **2** s %1.4f %1.4f\te %1.4f %1.4f\tr %1.4f %1.4f\n",
// armId,
// ths_act[0] RAD2DEG,validity2[0][0],
// the_act[0] RAD2DEG,validity2[0][1],
// thr_act[0] RAD2DEG,validity2[0][2]);
// /*
// printf("%7d d %0.4f \tp %0.4f \ty %0.4f\n",_ik_counter,
// d_act,thp_act RAD2DEG,thy_act RAD2DEG);
// printf("x (%f, %f, %f) z (%f, %f, %f) %f\n",xx,xy,xz,zx,zy,zz,cthe);
// printf("norms %f %f\n",x_roll_in_world.length(),z_roll_in_world.length());
// printf("(zy + kc12*kc23): %f (%f, %f)\n",(zy + kc12*kc23),zy, kc12*kc23);
// printf("(ks12*ks23): %f\n",(ks12*ks23));
// */
// }
report->success_ = true;
return report;
} else {
const float maxValidDist = 3 DEG2RAD;
float valid_dist[2];
for (int i=0;i<2;i++) {
float sum = 0;
for (int j=0;j<3;j++) {
float v = fabs(validity2[i][j]);
sum += v*v;
}
valid_dist[i] = sqrt(sum);
}
bool use0 = valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1];
bool use1 = valid_dist[1] < maxValidDist && valid_dist[0] > valid_dist[1];
printf("ik validity distances: (%s | %s) % 1.3f\t%f\n",use0?"Y":" ",use1?"Y":" ",valid_dist[0],valid_dist[1]);
if (valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]) {
printf("setting joints to ik soln 1\n");
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[0],the_act[0],thr_act[0]);
} else if (valid_dist[1] < maxValidDist) {
printf("setting joints to ik soln 2\n");
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[1],the_act[1],thr_act[1]);
}
// if ((_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d invalid **2** s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n",
// armId,
// ths_act[0] RAD2DEG,validity2[0][0] RAD2DEG,
// the_act[0] RAD2DEG,validity2[0][1] RAD2DEG,
// thr_act[0] RAD2DEG,validity2[0][2] RAD2DEG);
// printf(" s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n",
// ths_act[1] RAD2DEG,validity2[1][0] RAD2DEG,
// the_act[1] RAD2DEG,validity2[1][1] RAD2DEG,
// thr_act[1] RAD2DEG,validity2[1][2] RAD2DEG);
// }
return report;
}
return report;
}
