Ask Your Question
1

Trac IK cannot computeCartesian path at all

asked 2021-05-05 15:11:28 -0600

hopestartswithu gravatar image

updated 2021-05-06 07:55:52 -0600

I decided to give Trac IK a shot after urkinematics solver was too hard to implement on 2 ur10 robots.

It's "fine" I guess, aside from not being able to find IK once in a while but it is what it is.

My problem is I cannot get a cartesian path be it over the MotionPlanner Plugin on rviz or in python code. It just fails and gives this back

Computed Cartesian path with 1 points (followed 0.000000% of requested trajectory)

Also I would love to know what kind of parameters you guys set for the Trac IK solver. Would appreciate it if you have some tips for the UR10 robot too :)

EDIT:

More informations on my setup as wished!

So, I have 2 UR10 robots 1m distance in between them but I assume this doesn't really matter since KDL does solve cartesian path with no problems on both robots. Anyway, after planning with my python file a non cartesian path one of the UR robots has to rotate around a certain position. Meaning, I am just adding a certain amount of orientation on the z axis. Does as it should on KDL (aside from complete twists and jumps at full speed, forcing the robot to a certain place when not reachable at first).

Trac IK seems to have problems with the start state of the robots. After moving the robots to a certain point, replanning with cartesian paths fails even with the motionplanning plugin. If I set a new position with the interactive marker, plan and execute that (meaning I "refresh" the start position of the arms) I can plan with cartesian again but over the plugin.

After executing my code, planning with the plugin gives this error code:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'right_elbow_joint': expected: 2.21799, current: 1.7153

This is the section in which cartesian is planned:

        for cartesians in range(0, 2):

            rospy.sleep(1)

            self.move_group.set_start_state(self.move_group.get_current_state())
            self.move_group_left.set_start_state(self.move_group.get_current_state()) #planning for the right arm for now

            rospy.sleep(0.3)

            wpose = self.move_group.get_current_pose().pose
            wpose.orientation.z += self.pose_goal.pose.orientation.z/4

            self.waypoints.append(copy.deepcopy(wpose))

            cart_found = False

            while cart_found == False:
                (plan_cart, fraction) = self.move_group.compute_cartesian_path(
                                                self.waypoints,   # waypoints to follow
                                                0.01,        # eef_step
                                                0.0)         # jump_threshold

                quest = raw_input("Ausfuehren?(ja/nein): ")

                if(quest == "ja"):

                    self.move_group.execute(plan_cart, wait=True)

                    #self.sub_img =  rospy.wait_for_message('/camera/color/image_raw',Image, timeout=None)

                    #savedimage = self.saveImg(self.sub_img)
                    cart_found = True

        rospy.loginfo('Ausgefuehrt!')

my kinematics yaml:

arm_right:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  solve_type: Distance
arm_left:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  solve_type: Distance
edit retag flag offensive close merge delete

Comments

You'll want to make sure to configure Trac IK solve_type and set it to Distance. The default is Speed, which will make it return the first solution it finds -- which is not always the closest to the current state / the seed.

gvdhoorn gravatar image gvdhoorn  ( 2021-05-06 02:27:02 -0600 )edit

Did that already, still not working.Trac IK cannot perform the slightest amount of motion with cartesian. I have absolutely 0 clue why.

hopestartswithu gravatar image hopestartswithu  ( 2021-05-06 02:48:09 -0600 )edit

Then you'll have to describe your setup in much more detail for anyone to be able to help you.

Your current post is basically the equivalent of "it doesn't work".

Additionally: Trac IK does not "computeCartesian path". MoveIt executes that function. The IK solver is used by MoveIt, but the solver itself does not do any planning or create trajectories.

gvdhoorn gravatar image gvdhoorn  ( 2021-05-06 02:49:50 -0600 )edit

Good point, will edit it later! EDIT: I edited my post, also as a side comment: I removed the set_start_state functions out of "fun" and compute cartesian gives a path but because of state deviation it cannot be executed. I assume Trac IK cannot handle the end position of my robot well enough? But that would mean that I should be able to cartesian-plan extreme small motions with the plugin instead which doesn't either.

hopestartswithu gravatar image hopestartswithu  ( 2021-05-06 02:52:05 -0600 )edit

Meaning, I am just adding a certain amount of orientation on the z axis.

[..]

       wpose = self.move_group.get_current_pose().pose
       wpose.orientation.z += self.pose_goal.pose.orientation.z/4

pose.orientation is a quaternion, not an Euler angle triplet.

It's very unlikely you can simply add self.pose_goal.pose.orientation.z/4 to the current orientation. And even if it would result in a valid quaternion, it might not result in a rotation around the z axis.

The fact that KDL works at all surprises me.

gvdhoorn gravatar image gvdhoorn  ( 2021-05-06 08:10:01 -0600 )edit

I was actually working directly with quaternions and euler angles and this was just a random test. Extremely random. I was thinking "could I just do a rotation around the z axis for shits and giggles", well it worked. Anyway I am not really sure how to handle it because I didn't pay much attention to that after it worked like a charm. How could I force a rotation around the z axis? This is a complete different question but I am not sure if changing that would actually fix the problem mentioned above otherwise I could just stick to KDL with "worse" IK

hopestartswithu gravatar image hopestartswithu  ( 2021-05-06 08:40:53 -0600 )edit

I was actually working directly with quaternions and euler angles and this was just a random test.

So you're here to ask other people for support, first post a question with very little information, then add some information after it's been asked of you, but the information you give is then not what you're actually using. For "shits and giggles"?

Reason I'm hesitant to accept immediately that Trac IK is not able to solve for your UR10 is the readme stating a 99.33% solve rate for UR10s. Patrick Beeson was pretty thorough in his tests, so I'm inclined to believe it (but things could have changed in the meantime of course).

I would suggest to write a minimal (Python) script which shows your problem, and then posting that here. Perhaps people can replicate your problem and help you then.

gvdhoorn gravatar image gvdhoorn  ( 2021-05-06 08:48:17 -0600 )edit

I worded it extremely bad I guess. Sorry. I was working with quaternions for normal planning but couldn't use it for cartesian (I had another post where fvd helped me out alot with orientation and went with his solution). So I was testing random stuff because I didn't know any more and that seemed to work really well so I didn't dug deeper into quaternions. Once I got into IK problems I switched solvers. I really don't know if it's that part being problematic because, as I said, KDL didn't have any problems. I actually do use everything that's posted on top of the post and that's my current situation. Sorry again for writing that so bad.

I will write a small script and post it later (got too many uni lectures, sorry)

hopestartswithu gravatar image hopestartswithu  ( 2021-05-06 08:55:26 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-05-06 12:59:27 -0600

hopestartswithu gravatar image

While writing the python script I realised something very dumb: I had another frame set as end effector so the robots would always look at the given point/position of an object just by planning to it with the "new end effector." Works insanely well with KDL.

So I thought, I might as well set the end effector to the original tool tip frame and try that out and it suddenly worked! So I thought the extra frame is at fault. Well, that's not the case either. Apparently, for no reason at all (probably a reason I'm too blind to see) using Trac IK I have to define an end effector with "set_end_effector_link()" and setting it to my extra frame does the trick.

Adding the orientation just like I did above also works! I don't know if this is "bad" but I have yet to see something go wrong with that. In other words, rotating around a point like that seems to work too!

Side note: self.pose_goal.pose.orientation.z/4 doesn't make really sense, originally it was pi/x but out of desperation I had that changed for random values because even pose.position wouldn't work!

Thanks for helping out, gvdhoorn!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-05-05 15:11:28 -0600

Seen: 148 times

Last updated: May 06