[Nao MoveIt!] set goal points without orientations

asked 2022-07-21 08:58:00 -0500

matb gravatar image

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")

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_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!!!

edit retag flag offensive close merge delete


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?

matb gravatar image matb  ( 2022-10-12 05:54:11 -0500 )edit