Problem with Cartesian Paths

[ariyan kabir]

Hello,

I want a baxter arm's end effector to follow a certain cartesian path. I am following this tutorial for cartesian path planning.

Problem: The end effector only goes to the final way-point, without going through all the defined way-points. The points are valid and the arm can reach them as individual targets.

I am trying to use the cartesian path both on baxter gazebo simulator and on the physical robot, I am facing the same problem for both.

Please let know how to fix this issue! Thanks!

[Moises Estrada]

Hi Ariyan,

Can you post the code please.

Regards.

[ariyan kabir]

Hello Moises, the code is as below


import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg


moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
                anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("left_arm")
display_trajectory_publisher = rospy.Publisher(
                                    '/move_group/display_planned_path',
                                    moveit_msgs.msg.DisplayTrajectory)


waypoints = []
waypoints.append(group.get_current_pose().pose)

# A

wpose.position.x = 0.330587106437
wpose.position.y = 0.680298839472
wpose.position.z =0.509417521914

wpose.orientation.x = 0.886741133686
wpose.orientation.y = -0.461213970606
wpose.orientation.z = -0.0311102894965
wpose.orientation.w = -0.00199625526176

waypoints.append(copy.deepcopy(wpose))

# B

wpose.position.x = 0.319027463078
wpose.position.y = 0.696932535372
wpose.position.z = 0.38963384769

wpose.orientation.x = 0.89646727127
wpose.orientation.y = -0.441927661622
wpose.orientation.z = -0.0323472356087
wpose.orientation.w = 0.000172577279596

waypoints.append(copy.deepcopy(wpose))

# C

wpose.position.x = 0.31619260306
wpose.position.y = 0.699055646219
wpose.position.z = 0.327976309251

wpose.orientation.x = 0.896453836515
wpose.orientation.y = -0.441993767826
wpose.orientation.z = -0.0317790858886
wpose.orientation.w = 0.00145530078057

waypoints.append(copy.deepcopy(wpose))


(planABC,fraction)=group.compute_cartesian_path(waypoints,0.01,0.0)

group.go(planABC)

print "END"

[Moises Estrada]

I have never done Cartesians in python but the code looks good...
Do you know the final value of fraction?

On Tuesday, February 10, 2015 at 6:26:21 PM UTC-8, ariyan kabir wrote:

[ariyan kabir]

it shows: 0.9675324675324676

do you have any cartesian path code successfully running on baxter or any other robot in gazebo? 

Thanks!

[Moises Estrada]

I have not tried with a baxter but here is an example code and video of my robot:

https://github.com/moicez/pentaxis/blob/master/cartesian_trajectories/src/sine.cpp

https://www.youtube.com/watch?v=Gl_mZR0w330

[Moises Estrada]

I forgot to mention that fraction should be 1.0 for the correct execution of the trajectory.

[ariyan kabir]

Thanks Moises!

so, f<1 means its failing to compute the trajectory? what could be the possible reasons? does the starting pose matter?

[Moises Estrada]

Correct, f<1 means that it fails to compute the trajectory.

There could be different reasons why it fails, it could be that one of the suggested pose is not possible for the robot to do,
also the pose could be outside its work space, maybe a possible collision or that the kinematics fails to calculate the inverse kinematics
for that pose.

I would recommend the following:
1. Try to do a trajectory that you know a priori that it is doable by your robot.
2. Turn on the debugger option when you launch you launch file.

What kinematic solver are you using? I had problems doing cartesians when using the KDL solver, so that is why I opted to do an analytical one.

[ariyan kabir]

I have tested the chosen A,B,C points manually. A-B-C, A-B, B-C, C-A combinations are valid when done manually setting one target pose at a time. 

But yes, I am using the default KDL solver!

[Moises Estrada]

I suspect that numeric ik solvers like the KDL are not well suited for cartesians.

One thing to take into account is that even though the poses A, B and C are solvable, does not mean that the in between points are.

[ariyan kabir]

I am quite new with ros, moveit and motion planning. could you please guide me how to choose and install a suitable kinematics solver?

Thanks!

[Moises Estrada]

I'm afraid I can't be of much help there, you could try IK fast:

http://wiki.ros.org/Industrial/Tutorials/Create_a_Fast_IK_Solution

Maybe there is one already made for the popular baxter.

[ariyan kabir]

i have installed ik fast and trying to use it, but facing somewhat the same problem. moreover, it seems to be less accurate to reach individual targets :/

Is there any methodical way to set target points such that they will be reachable for sure?

[Daniel Solomon]

Actually, numerical solvers are better at this Cartesian motion problem than the IKFast plugin, so I would stick with KDL.

The fact that you can find IK for each point individually in no way guarantees that you can move between those points with straight-line motion.

The value of your fraction variable means that you have successfully planned 96.8% of your path, which at a quick glance seems to be about 180mm in length from A-C. If the arm's current location is very near A, then that would mean your plan stops ~6mm from the final goal.
How close to A is the arm in it's initial pose? Also, even though the plan is incomplete, planABC should still be populated with 96% of the plan. You can calculate how close you are to the goal, or even display it in RViz (it takes a little bit of effort).

W.R.T. your last question, that is a common motion planning conundrum. You can quickly check that each individual pose is reachable, but I don't know of any concrete way to test that a direct path between poses is possible without trying to plan it.

[ariyan kabir]

Does people use their own kinematics solver in most of the cases then? 

What is the significant use of moveit library then? 

[Daniel Solomon]

I believe that most people use KDL. It works for a large variety of cases, and is very easy to use with new robots, but it does not have the performance of an analytic solver like FastIK.

Moveit is a framework for solving motion planning problems. It was built to solve difficult planning tasks that are solved with libraries like OMPL.

The computeCartesianPath functions are an ancillary capability that are provided as a convenience, but there is not a high level of sophistication in its operation. You are only required to use moveit to computer Cartesian paths because there is no python interface to the RobotState class that provides that functionality.

[ariyan kabir]

Please correct me if I am wrong about the followings. I am learning! 

the kinematics solver - computes the cartesian path. i.e. - determines if it is actually possible to travel through the points.

OMPL library - plans that path 

moveit - just an interface to operate the robot or implement the motion plans on the robot. also, moveit provides some extra benefits of collision avoidance and helping compute the cartesian path using the chosen kinematics solver 

now,

Fundamental (Noob) question: if i wish my robot's end effector to travel along a path on some surface, what should be my step-by-step approach to accomplish the task?

[ariyan kabir]

Hello everyone, now I am trying a new path and for which I am getting fraction=1. also, i can see the plan in rviz, but it does not execute in gazebo!  Could you please identify if i am making any mistake?
i am using the group.go() command for execution. the code is following:

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

import math


moveit_commander.roscpp_initialize(sys.argv)

rospy.init_node('move_group_python_interface_tutorial',
                anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("left_arm")
display_trajectory_publisher = rospy.Publisher(
                                    '/move_group/display_planned_path',
                                    moveit_msgs.msg.DisplayTrajectory)

group.clear_pose_targets()

bx=0.319027463078
by=0.696932535372
bz=0.38963384769
#pose B
pose_target=geometry_msgs.msg.Pose()
pose_target.position.x=bx
pose_target.position.y=by
pose_target.position.z=bz

pose_target.orientation.x=0.89646727127
pose_target.orientation.y= -0.441927661622
pose_target.orientation.z=-0.0323472356087
pose_target.orientation.w=0.000172577279596

#pose_target.position.z=bz+.05
group.set_pose_target(pose_target)
poseB=group.plan()
group.go(wait=True)


ee=.1
theta=0
zz=bz
sign=1

waypoints = []
wpose=geometry_msgs.msg.Pose()

wpose.position.x = 0.319027463078
wpose.position.y = 0.696932535372
wpose.position.z = 0.38963384769

wpose.orientation.x = 0.89646727127
wpose.orientation.y = -0.441927661622
wpose.orientation.z = -0.0323472356087
wpose.orientation.w = 0.000172577279596

waypoints.append(copy.deepcopy(wpose))

while theta<=90:
    byee=math.cos(math.radians(theta))
    bxee=math.sin(math.radians(theta))
    wpose.position.x=bx+ee*bxee
    wpose.position.y=by+ee*byee
    waypoints.append(copy.deepcopy(wpose))
    theta+=5
    print theta

(plan_circ,fraction)=group.compute_cartesian_path(waypoints,0.01,0.0)

group.go()

[ariyan kabir]

is there any different execution command rather group.go() ???

[Steffen P]

Yes, you just have to call group.execute(plan_circ) to execute the plan you calculated with compute_cartesian_path.

See here for a complete example.

[Daniel Solomon]

Ariyan,
I just wanted to clariAustin question you asked a few days back. The Cartesian solver is actually implemented in the RobotState class. It does not use OMPL at all. OMPL, in the context of MoveIt, is a set of libraries that is often used for complex planning tasks in joint space, not Cartesian space.

MoveIt itself is a framework for motion planning. It establishes a common pipeline for managing planning tasks, and it enables all the different pieces of planning, including collision detection, to work together.

To help with Cartesian planning tasks, the moveit simple interface (that is what is typically used for these simpler problems) provides a service to plan the Cartesian path. This enables Cartesian planning from python, because there is no python access to the robot state class. It does not use the typical moveit pipeline, however, and can't really be compared to OMPL style planning tasks.

[ariyan kabir]

Thanks a lot Steffen! ^_^

[ariyan kabir]

that explains! thanks!