Changed the way to compute the orientation. It only works if the desired pointing axis is x.

config/joints_module_default.yaml

@@ -5,7 +5,7 @@ joint_states_watchdog_time_s: 1.0
 
 pointing_frame: "head_camera_link"
 pointing_axis: 0
-point_frame: "base_link"
+point_frame: "torso_lift_link"
 pt_enabled: True
 
 pt_max_retries: 1

src/iri_joints_module.cpp

@@ -421,6 +421,7 @@ bool CIriJointsModule::point_to(double x, double y, double z, double max_vel)
     this->unlock();
     return false;
   }
+  std::cout << "point to: " << x << "," << y << "," << z << std::endl;
   this->point_to_goal.target.point.x = x;
   this->point_to_goal.target.point.y = y;
   this->point_to_goal.target.point.z = z;