[Nao MoveIt!] set goal points without orientations
Hi everyone, I'm trying to move Nao's arms using 3D cartesian points taken from camera. (Ubuntu 20.04 + ROS Noetic + python).
I'm using nao_moveit_config (launched with roslaunch nao_moveit_config demo.launch
) as the Rviz/Nao config.
The following is the code used in my python module.
Let's say I have a goal point (the reference frame used is the same as "base_link" and the point is reachable because i read it from the current pose and then i "manually" move the arm in a random position in Rviz, to see if the arm moves correctly. In fact if I copy also rotations, it works):
goal = geometry_msgs.msg.PoseStamped()
goal.header.frame_id = "base_link"
goal.pose.position.x = 0.025002952528081792
goal.pose.position.y = -0.18831814132862237
goal.pose.position.z = 0.2273321576174629
and I want to use this point to move the arm of Nao:
mg = moveit_commander.MoveGroupCommander("right_arm")
...
mg.go(wait=True)
But here start my problems:
1) I don't have rotation for my points since I use 3D cartesian point from camera (inner question: is there a way to calculate the end point orientations from let's say the previous points in the chain?). As a result, the only function that works for me is
mg.set_joint_value_target(goal, arg3=True)
the other functions, such as:
mg.set_joint_value_target(goal)
mg.set_pose_target(goal)
mg.set_position_target([00.025002952528081792,-0.18831814132862237, 0.2273321576174629])
don't work at all, they don't move anything. I cannot explain why.. (Of course these functions are in place of ...
above).
2) Even with mg.set_joint_value_target(goal, arg3=True)
the end-effector of the arm (the wrist) doesn't reach the point. It seemed to me like either a planning or a tolerance issue so I started trying various goal tolerances, precisions, and various planners...without any result.
It is difficult to me also reporting the behaviour since it doesn't behave always in the same way: sometimes the wrist reaches the point, sometimes not so much (tolerance issue??) and in addition with strange arm configurations (planner issue??). Since the point I use is the same for all the tests I made, it seems to depend on the start position of the arm..is that possible? Is it a problem with orientations?
What am I doing wrong?
Sorry for the length of this message, I hope that all is clear. This situation is driving me mad, please help me!!!
I've tried the same thing (using
set_position_target
) with the panda robot and it was able to reach the points without the need to specify orientations for those points. It might be a problem with the specific urdf of the NAO?