Problems with moving the Ur16e Robot with inverse differential kinematics

edit

Inverse differential Kinematic - Problem with Orientation

Hey guys,

currently I am working to control my UR-Robot. My goal is to control the robot with inverse differential kinematics : 1.) vel_q = J^-1 * (x_goal - x_act)

I am able to get the actual position via Moveit Service GetPositionFK. I translate the quaternion to euler with tf_transformation.

Furthermore I subscribe and publish the jacobian matrix with movegroup_commander.

My problem is that the Robot is moving (gazebo) to the target position (x,y,z) but does not find the correct orientation. The result is that the robot goes literally crazy.

Can somebody tell me maybe what is my mistake or what can I improve?

Thanks in advance!