Re-implementation of trajopt in python by John Schulman. The original document can be found in here: http://rll.berkeley.edu/trajopt/doc/sphinx_build/html/#

Trajectory planner using Sequential Quadratic Programming (SQP). Refer examples inside scripts directory to know how to use this trajectory planner

The black ink represents the Class name and the blue represents the function name.

planner_example_kukka_arm_without_app.py

Setting up the Trajectory Optimization Planner

self.planner = TrajectoryOptimizationPlanner(**config)

This additionally initiates the Robot and the SimulationWorld class which can be accessed using self.planner.robot and self.planner.world

Weirdly, the robot is made a member of the world member object of the planner object.

self.planner.world.robot = self.planner.robot

self.planner.world.toggle_rendering(0) Basically p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, enable)

Then the code adds a plane, a table and a box on the table

Now we enter the run() function.

Author has hard coded 11 joint states in a config .yaml file which they use to randomly select a joint config from using start = randint(1, 11) end = randint(1, 11)

self.planner.reset_robot_to(start_state, group) eventually calls the p.resetJointState()

Now comes the crux of this example -

status, is_collision_free, trajectory = self.planner.get_trajectory(group=group, start_state=start_state,
                                                                                goal_state=goal_state, samples=samples, duration=duration,
                                                                                collision_safe_distance=collision_safe_distance,
                                                                                collision_check_distance=collision_check_distance
                                                                                )

Now's the right time to refer to the Function call DFS tree to see what this calls. Note that group = "full_arm", samples = 20, duration = 20, collision_check_distance = 0.15 , collision_safe_distance = 0.1.

1. is_given_state_in_collision() of the SimulationWorld class is called to check if the start and the goal configurations are collision free (both self and environmental)
2. init_plan_trajectory() of the Robot class is called which calls the init() method of the Planner class which in turn calls the init() of ProblemModelling, SQP_Solver and Trajectory classes. [More details to come]
3. init() of ProblemModelling takes in joints, no_of_samples, duration, decimals_to_round, collision_safe_distance, collision_check_distance and fills in the cost, start-goal and constraints matrices. Note that the above also calls a method fill_velocity_limits() that initiates a initial guess with a linear interpolation (with 20 samples in the Kuka example) among other things.
4. self.sqp_solver.solve(initial_guess, callback_function) is then called with initial_guess as a linear interpolation of 20 samples between the start and the goal state and callback_function = callback_function_from_solver() from TrajectoryOptimizationPlanner class.
5. In the sqp_solver.solve() the trust region method parameters are set from sqp_config.yaml. After that it starts the trust region method and calls the callback_function_from_solver() function with the new_trajectory = the initial guess with size 140 and delta_trajectory = list of size 140 with zeroes.