bool IKSolver::solveIK(const string &tipLinkName, const Eigen::Vector3d &pos, const Eigen::Quaterniond &orient, const JointState &cs, JointState &state)
{
using namespace Eigen ;
assert(model_) ;
Affine3d solver_tip_to_pose_tip = model_->getWorldTransform(solver_->getTipName()).inverse() * model_->getWorldTransform(tipLinkName) ;
Affine3d frame = model_->getWorldTransform(solver_->getBaseName()) ;
Affine3d pose_ = frame.inverse() * Translation3d(pos) * orient * solver_tip_to_pose_tip.inverse() ;
geometry_msgs::Pose pose = eigenPoseToROS(pose_.translation(), Quaterniond(pose_.rotation())) ;
vector<double> solution ;
int error_code ;
vector<double> seed_state_vals = cs.getValues(joint_names_) ;
if ( solver_->searchPositionIK(pose, seed_state_vals, 1.0, solution,
boost::bind(&IKSolver::initialPoseCheck, this, _1, _2, _3),
boost::bind(&IKSolver::collisionCheck, this, _1, _2, _3),
error_code ) )
{
state = JointState(joint_names_, solution) ;
return true ;
}
else return false ;
}
void trace_rays0(MData *m, SurfaceList *surflist, int nsurfs, Ray *R, int nray, int rsize, char *xray)
{
int *traversed = new int[nray];
Ray *ray;
//fprintf(stderr, "Ray0 %ld %p %d %d\n", R, R, nray, rsize);
//fflush(stderr);
//return;
for ( int h = 0; h < nsurfs; h++ ) {
Surface *surf = surflist[h].surf;
int nsurf = surflist[h].nsurf;
int once = surflist[h].type;
int j;
if ( once ) { for ( int j = 0; j < nray; j++ ) { traversed[j] = 0; } }
for ( int i = 0; i < nsurf; i++ ) {
if ( isinf(surf[i].p[Px_thickness]) ) { continue; }
if ( !surf[i].enable ) { continue; }
Affine3d txforward = Affine3d::Identity();
Affine3d txreverse ;
Affine3d rtforward ;
Affine3d rtreverse ;
txforward = // Ray position transform
Affine3d::Identity()
* Translation3d(0.0, 0.0, m->z)
* AngleAxisd(d2r( surf[i].p[Px_rz]), Vector3d(0.0, 0.0, -1.0))
* AngleAxisd(d2r(-surf[i].p[Px_ry]), Vector3d(0.0, 1.0, 0.0))
* AngleAxisd(d2r(-surf[i].p[Px_rx]), Vector3d(1.0, 0.0, 0.0))
* Translation3d(0.0, 0.0, -m->z)
* Translation3d(-surf[i].p[Px_px], -surf[i].p[Px_py], -surf[i].p[Px_pz])
;
rtforward = // Ray direction transform
Affine3d::Identity()
* AngleAxisd(d2r( surf[i].p[Px_rz]), Vector3d(0.0, 0.0, -1.0))
* AngleAxisd(d2r(-surf[i].p[Px_ry]), Vector3d(0.0, 1.0, 0.0))
* AngleAxisd(d2r(-surf[i].p[Px_rx]), Vector3d(1.0, 0.0, 0.0))
;
txreverse = txforward.inverse();
rtreverse = rtforward.inverse();
//printf("Surface %s %d %d: %f %f %f %d %ld\n", surf[i].type, h, i, -surf[i].p[Px_px], -surf[i].p[Px_py], -surf[i].p[Px_pz], once, surf[i].traverse);
for ( j = 0, ray = R; j < nray; j++, ray = (Ray *) (((char *) ray) + rsize) ) {
Vector3d saveP = ray->p;
Vector3d saveK = ray->k;
//printf("Ray ");
//prays(ray, 1);
if ( ray->vignetted ) { continue; }
ray->p = txforward * ray->p; // Put the ray into the surface cs.
ray->k = rtforward * ray->k;
//printf("Conv ");
//prays(ray, 1);
ray->vignetted = surf[i].traverse(m, &surf[i], ray);
//printf("Trav ");
//prays(ray, 1);
if ( ray->vignetted == 2 ) { // Coordbreak returns 2
ray->vignetted = 0;
if ( xray ) {
memcpy(xray, ray, rsize);
xray += rsize;
}
continue;
}
if ( ray->vignetted || (!ray->vignetted && aper_clip(&surf[i], ray)) ) {
ray->vignetted = i ? i : -1;
}
if ( once ) {
//printf("Here ");
//prays(ray, 1);
if ( !ray->vignetted ) { // If the ray was not vignetted it has traversed this surface.
//printf("Trav ");
//prays(ray, 1);
traversed[j] = 1; // Don't try this ray again
ray->vignetted = 1;
} else {
//printf("Ving ");
//prays(&ray[j], 1);
ray->p = saveP; // Reset
ray->k = saveK;
ray->vignetted = 0; // Try again on next surface
continue;
}
}
ray->p = txreverse * ray->p; // Put the ray back into global cs..
ray->k = rtreverse * ray->k;
if ( xray ) {
memcpy(xray, ray, rsize);
xray += rsize;
}
//printf("Next ");
//prays(ray, 1);
}
if ( !once ) {
m->indicies = surf[i].indicies[0] > 0.0 ? surf[i].indicies: m->indicies;
m->z += surf[i].p[Px_thickness];
}
}
//printf("Done ");
//prays(ray, 1);
if ( once ) {
for ( j = 0, ray = R; j < nray; j++, ray = (Ray *) (((char *) ray) + rsize) ) { // Rays that have not traversed are vignetted.
ray->vignetted = !traversed[j];
}
m->indicies = surf[0].indicies[0] > 0.0 ? surf[0].indicies: m->indicies;
m->z += surf[0].p[Px_thickness];
}
}
delete [] traversed;
}
