Inverse differential Kinematic - Problem with Orientation
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? Is the method x_target - x_act = error wrong? Should I maybe use x_act - x_previous / t_act - t_previous to calculate the tcp velocity?
Thanks in advance!