search

justagist / franka_ros_interface

A ROS/Python API for controlling and managing the Franka Emika Panda robot (real and simulated).
http://justagist.github.io/franka_ros_interface
Apache License 2.0
171 stars 59 forks source link

Trouble in executing a plan using move group interface #23

Closed karthikm-0 closed 3 years ago

karthikm-0 commented 3 years ago

Hi,

I'm having a really strange issue when executing plans with the move group interface in Gazebo. I have a set of waypoints containing joint value targets (generated by a DMP) that I want the robot to execute. I take these sets of targets and create a RobotTrajectory plan which is then sent to the move group interface to execute the plan (using the execute_plan method). However, for some odd reason, I get the following error:

JointTrajectoryActionServer: Exceeded Error Threshold on panda_jointx.

Upon further inspection, the threshold error it shows is between the first waypoint target value and the final waypoint target value. For instance, if this is the first and last target:

first: (0.5506333239656609, -0.5516712630390663, 0.07962400626558634, -2.2278973029167877, 0.10920540955632561, 1.678958868214459, 1.5042823665087401)

last: (0.6314878008421038, -0.5918658233568661, 0.34556952933794505, -2.221073606844203, 0.28302357696594, 1.6974351499160627, 1.921374168797591)

The error that occurs would be "JointTrajectoryActionServer: Exceeded Error Threshold on panda_joint3: 0.266121303193". However, there are several waypoints between the start and end that need to be executed. It seems that the action server is attempting to move to the final target without attempting to move to any of the intermediate targets before it. I suspect it is because the generated waypoints do not have a time associated with them, which could be confusing the server. Could you provide some guidance on why this is occurring? Thanks!

EDIT: After adding a time_from_start to each point, now the trajectory is completely random and does not reach the end goal and reaches some unknown target.

The only approach that seems to work consistently is using plan_joint_path but this is a blocking method and the resulting trajectory is painfully slow (each point has its own plan).

The other approach seems to be to publish joint commands manually for each point and then sleep for a short duration.

justagist commented 3 years ago

Hi, Depending on the number of waypoints you want the robot to follow, it can get very difficult to get a successful and feasible plan using Moveit, especially if you are using a time-indexed trajectory like the ones generated by DMPs. Moveit by default will (try to) use its own planners to create a new path plan for the robot to follow while staying within the constraints you specify (eg. waypoints). This may not necessarily succeed, and if it does there is no guarantee that it takes the path you want.

If you have a trajectory encoded as a DMP, you can have a continuous stream of goal positions (discretised for each time step). It is better to use a controller to follow this trajectory, rather than depending on moveit (if you want to follow the DMP path, then Moveit is unnecessary, less reliable and slower). I would suggest generating a smooth trajectory from the DMP and commanding the robot joints directly with the generated trajectory points. Since DMP allows for generating discretised trajectories with any level of granularity, you can generate points that are close enough to provide a smooth trajectory and ensure that the robot is following the required path. The safest way would be to write a control loop that would command the robot joints to the required joint position targets at each time step using the time-indexed trajectory generated by the DMP.

Also, just to be clear, it should be possible to execute a plan through waypoints using Moveit as long as the provided points are within the accessible workspace for the robot. There are several things you have to consider here though (eg. number of waypoints and time constraints would increase the planning complexity). There are several resources online which can help you with using Moveit for planning through multiple waypoints. This package is not implementing or providing support for planners, but only provides an interface for using MoveIt. I can't provide much support if the problems you are having are with MoveIt itself.

Hope this helps.

-- Saif

karthikm-0 commented 3 years ago

Thank you very much for the detailed explanation. My assumption was that I could create my own plan which the planner would attempt to follow but I suppose the constraints are maybe too stringent. I ended up writing a control loop and it is quite smooth.

justagist commented 3 years ago

A 'planner' by definition would only create a plan for the robot to follow and by itself does not make the robot 'follow' any plan. You should have a 'controller' for this purpose. Moveit works this way too. It creates a plan using one of its available path planners, and then uses a controller to execute it. When using moveit, this controller is by default one of the ROS TrajectoryControllers. So if you are creating a plan yourself, you don't have to (should not) provide it to a moveit planner again, but should just choose an appropriate controller to execute your plan (while ensuring stability and smoothness ofc).

karthikm-0 commented 3 years ago

Sorry, I misspoke! I meant when providing a pre-made plan to execute_plan which in turn would request a controller to execute it. But this makes sense, thanks!