Cannot move robot to a custom pose with Moveit_commander

edit

Hello,

My goal is very simple: I would like to move my arm robot to a custom pose (position and orientation). The problem is that at the moment, it seems impossible...

For a better contextualisation: my robot is a Moveo BCN3D. You can find the files I use to define the virtual robot here: https://github.com/jesseweisberg/move.... I started to work from his project, as a base. Also, I'm working with ROS Kinetic on Ubuntu 16.04.

Then, I used the MoveGroupPython Interface : https://github.com/ros-planning/movei... because my goal is to command the robot with a defined pose.

After that, I tried to use the "go_to_joint_state()" function which works with my custom link data. Besides, if I move the virtual robot on Rviz (in the planning section), then I get the actual pose of the robot with the "get_current_pose()" function and I store it, I can move the robot to this pose again with "go_to_pose_goal()". This is great! But the problem is that I only success to perform a movement with a pose the robot has already reached once :/ If now, I change the pose a little bit, it doesn't not find any solution.

Example:

This pose (already reached once in Rviz) success to be reached by the robot with the Python script:

  pose_goal.pose.position.x = -0.0114960146688
  pose_goal.pose.position.y = 0.00848029943841
  pose_goal.pose.position.z = 0.24122011389
  pose_goal.pose.orientation.x = -0.609566357411
  pose_goal.pose.orientation.y = -0.660055156044
  pose_goal.pose.orientation.z = -0.301695665963
  pose_goal.pose.orientation.w = 0.318960455279

But this pose (a little bit modified by me, but still in the work envelope) can't be performed by the robot:

  pose_goal.pose.position.x = -0.01
  pose_goal.pose.position.y = 0.01
  pose_goal.pose.position.z = 0.24
  pose_goal.pose.orientation.x = -0.609566357411
  pose_goal.pose.orientation.y = -0.660055156044
  pose_goal.pose.orientation.z = -0.301695665963
  pose_goal.pose.orientation.w = 0.318960455279

Actually, on the terminal I get this error: "ABORTED: No motion plan found. No execution attempted". And on the Rviz terminal, I get: "arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree"

But why can't the robot reach this position?! (it just doesn't make any sense)

It hasn't found any planning solution. So I tried several ways to fix this issue. My first trial was to change to change the solver from KDLKinematicsPlugin to IKFastKinematicsPlugin. Then, to modify the planner config (RRTkConfigDefault, SBLkConfigDefault, PRMkConfigDefault...). I also tried to add some tolerance to the final position (with the set_goal_orientation_tolerance() and set_goal_position_tolerance() functions). I also noticed that the orientation data parameters are normalized so we have to ensure that the square sum of them is equal to 1. And finally, my idea was to extend the planning time to give to the solver more time to calculate. But nothing works at all!

I have no idea about how to solve this. Please let me know if you have any suggestion to help ... (more)