Hi community,

I am playing around with my custom 5 DoF robot and the "movegrouppythoninterface". Basically, I am just changing the posegoal.position in the gotopose_goal() function like this:

current_pose = self.move_group.get_current_pose().pose
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = current_pose.position.x-0.1
pose_goal.position.y = current_pose.position.y
pose_goal.position.z = current_pose.position.z

As you can see, I am just trying to move my EE to -0.1 along the x-axis. Then I get this error

ABORTED: No motion plan found. No execution attempted.

What I can think of is that my EE tries to move in - x_axis direction without changing the orientation and since the robot only has 5 DoF, it fails. Currently, I am using the KDL solver (it is known that this solver only works when the robot has >= 6 DoF but when I tested it in RViz with this gumball it somehow manages to find ik solutions, so I am just using it.... ).

My question is since the reachability of my robot is very limited I would like to move the EE to the nearest point when the given pose_goal.position is not kinematically reachable. How could I do this? Is it even possible with this iterative KDL solver?