Continuous replanning and execution

[Stefan Kohlbrecher]

Hello,

I'm currently looking into making planning more robust for a floating base platform such as a humanoid, where the robot planning frame might move with regards to the world frame during plan execution due to disturbances and/or balance control. Other examples where this could be useful are a quadrotor with a manipulator or a robot trying to follow a moving object with the endeffector.

One idea I wanted to experiment with is performing continuous replanning for reaching a cartesian goal frame with the endeffector. This is is somewhat different from the current replanning approach in moveit that checks for validity of the remaining trajectory and replans to a planning goal that is not modified. Instead, in my scenario, I'd need to modify the planning goal (by calling IK based on current world state) and then replan.

The other thing I'd like to have is continuous execution that updates the executed trajectory as soon as a new plan has been computed. While not completely trivial to make this smooth in all situations, the joint_trajectory_controller supports updating (future) parts of trajectories in principle and with proper timestamping and interpolation, it should be possible to get smooth motion while continuously replanning in the background. After looking at the code, it looks like this cannot be done with the TrajectoryExecutionManager currently however, as it does not support updating trajectories (instead it can either be stopped before finishing execution of a trajectory, or it will call a callback when it finished executing).

The pipeline from planning to  arrival of trajectories at joint_trajectory_controllers is pretty complex, so I'm currently pondering hacking a shortcut vs. properly integrating with the existing execution pipeline. Any discussion, suggestions or hints are appreciated.

regards,

Stefan

[Stefan Kohlbrecher]

After looking into this some more it looks like pushAndExecute (https://github.com/ros-planning/moveit_ros/blob/indigo-devel/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h#L164) actually already supports most of what I'm looking for.

[Scott Paulin]

Thanks for posting your solution! I have been trying to figure this one out too!

[ust...@gmail.com]

Hello Stefan,
may I ask you about the current replanning approach in moveit?
I add a floating joint to my robot, but it will hit the new obatacle even the obstacle has been updated in octomap.

regards

[rohit kalaskar]

Hello Stefen,
I am trying to replace the trajectories online(while executing another trajectory). I don't think moveit allows such online trajectory replacement. Implementation: https://github.com/ros-planning/moveit_ros/blob/indigo-devel/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp#L308
Also, https://github.com/ros-planning/moveit_ros/blob/indigo-devel/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp#L230
I keep getting error "[ERROR] [...]:Cannot push a new trajectory while another is being executed" when I'm using asyncExecute().
As you said Joint trajectory controller supports trajectory replacement http://wiki.ros.org/robot_mechanism_controllers/JointTrajectoryActionController#Advanced_behaviors. Just not sure if Moveit implements it.
Am I missing something here ? Can you please help if you have figured it out already.
Thanks in advance.

[Stefan Kohlbrecher]

I didn't finish my continuous replanning implementation, but a video of work in progress is available here:

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

I'm not sure how much of that jerkiness is coming from badly tuned Gazebo controller gains and how much is related to trajectory replacement. The code I used is available here:

https://github.com/team-vigir/vigir_manipulation_planning/blob/master/vigir_plan_execution/src/continuous_plan_execution.cpp

It is highly experimental and essentially selects a random joint configuration and goes there while continuously replanning (and re-running IK every replanning iteration).

[rohit kalaskar]

Thanks Stefan,
I was trying to send trajectories directly to the controller using publisher with future time stamp. But time stamp doesn't seem to have any effect; Trajectories get executed right after publishing. May be something is wrong with the controller.

[Stefan Kohlbrecher]

If you use the joint_trajectory_controller, this should not happen as explained here: http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement

If you use some custom controller, there of course is no guarantee it correctly performs trajectory replacement.

[Teresa Seco]

Hi Stefan,

I had a look to your code continuous_plan_execution.cpp and I think it would solve my issue about trying to send a new trajectory when the previous one has not finished yet (trajectory replacement):

https://groups.google.com/forum/#!searchin/moveit-users/teresa$20seco/moveit-users/ZkXCZT7M_gA/TMmuLGQJBAAJ

I am working with move_group and asyncExecute but it does not work, and error arises:

[ERROR] [1435841315.524423483]: Cannot push a new trajectory while another is being executed

I would like to check with pushAndExecute from trajectory_execute_manager but I do not know how to obtain a move_group::MoveGroupContextPtr context linked to my group (moveit::planning_interface::MoveGroup("manipulator")) or how to obtain the trajectory_execute_manager object from my group.

Any suggestions?

Thank in advance!

[Dina Youakim]

Hi Stefan,

I am really curious to understand how you interface MoveIt! controller manager with your ros_controllers (joint_trajectroy_controller).
I am so much confused, as I wish to have the trajectory replacement strategy explained on their website, but I don't know exactly how to reach this behavior integrating it with MoveIt!.
At the end on MoveIt! side I have to provide a controller_manager (which is basically an action lib client),and then I have to write an action server to deal with the low level controllers.
Even I have done this already and I have modified the trajectory execution manager in MoveIt! to accept more than one trajectory, what happened on the server side that it first finishes the first one and then start executing the new one.
I am so much confused, the problem is in my implementation of the server side or in MoveIt! controller or MoveIt! trajectory execution manager.

Thanks,
Dina.

[Manuel Rodriguez]

Am Mittwoch, 16. September 2015 11:38:22 UTC+2 schrieb Stefan Kohlbrecher:

> I'm not sure how much of that jerkiness is coming from badly tuned Gazebo controller gains and how much is related 

> to trajectory replacement. 

Jerkiness is a common problem. The reason ist, that the reward function selects a new goal even the old goal was good enough.