Remove implicit 180 degree rotation of target pose

[marip8]

The reach study currently implicitly rotates each target pose by 180 degrees about the x-axis and makes the assumption that a robot's TCP frame has the z-axis pointing "out". This assumption and implict operation can be easily missed, causing reach studies to report many infeasible IK solutions, which is generally difficult to debug. We should remove the implicit rotation and state the assumption that the reach study will attempt to align the TCP frame directly with the target poses (per the operation of the underlying IK solver).

reach/src/reach_study.cpp

Line 111 in d89ddaf

const Eigen::Isometry3d& tgt_frame = target_poses_[i] * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());

[nikhil-sethi]

When this rotation is removed, the IK solver does not give any solutions though.

I'm able to get the same set of solutions (with the targets flipped) when I flip the tgt_frame before passing to evauateIK. Does this mean that the IK solver itself expects opposing z axes?

std::tie(solution, score) = evaluateIK(tgt_frame* Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()), params_.seed_state, ik_solver_, evaluator_);

[marip8]

When this rotation is removed, the IK solver does not give any solutions though.

You won't be able to just remove this rotation alone. You'll also need to rotate the TCP frame 180 degrees in your URDF to compensate.

The idea here is that the visualization should show the TCP frame aligning directly with the visualized poses, which is what most people assume when they think about inverse kinematics

[nikhil-sethi]

Yep I agree with the idea and also updated the urdf but the observations are a bit different.

When I remove the implicit rotation (z axis of target frames inside the hood), and update the URDF (z-axis of tcp inward), I do get solutions. But the visualizations show that they are not aligned. They are opposing.
All this is without the hack that i mentioned in the previous comment

Basically, I can only get solutions when the frames are opposing each other. But it's likely that I'm messing up the logic somewhere. Let me know!

[marip8]

Looks like there is a corresponding z-axis flip in the visualization of the markers in the reach_ros2 package that needs to be removed as well

[nikhil-sethi]

Hey @marip8, I've added #90 and ros-industrial/reach_ros2#46 to fix this.