Trac IK cannot computeCartesian path at all
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
You'll want to make sure to configure Trac IK
solve_typeand set it toDistance. The default isSpeed, which will make it return the first solution it finds -- which is not always the closest to the current state / the seed.Did that already, still not working.Trac IK cannot perform the slightest amount of motion with cartesian. I have absolutely 0 clue why.
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.
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.
pose.orientationis a quaternion, not an Euler angle triplet.It's very unlikely you can simply add
self.pose_goal.pose.orientation.z/4to 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.
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
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.
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)