skrobot.planner.sqp_plan_trajectory

skrobot.planner.sqp_plan_trajectory(collision_checker, av_start, av_goal, joint_list, n_wp, safety_margin=0.01, with_base=False, weights=None, initial_trajectory=None, slsqp_option=None, optimization_callback=None, base_limit=inf)[source]

Gradient based trajectory optimization using scipy’s SLSQP.

Collision constraint is considered in an inequality constraints. Terminal constraint (start and end) is considered as an equality constraint.

Parameters:

  • av_start (numpy.ndarray(n_dof,)) – joint angle vector at start point

  • joint_list (list[skrobot.model.Link]) – link list to be controlled (similar to inverse_kinematics function)

  • n_wp (int) – number of waypoints

  • safety_margin (float) – safety margin in collision checking

  • with_base (bool) – If with_base=False, n_dof is the number of joints n_joint, but if with_base=True, n_dof = len(joint_list) + 3.

  • weights (numpy.ndarray(n_dof,) or None) – cost to move of each joint. For example, if you set weights=numpy.ndarray([1.0, 0.1, 0.1]) for a 3 DOF manipulator, moving the first joint is with high cost compared to others. If set to None it’s automatically determined.

  • initial_trajectory (numpy.ndarray(n_wp, n_dof) or None) – initial solution in the trajectory optimization specified by a angle vector sequence. If None, initial trajectory is automatically generated.

  • slsqp_option (dict or None) – option of slsqp. Please see options in https://docs.scipy.org/doc/scipy/reference/optimize.minimize-slsqp.html for the detail. If set to None, a default values is used.

  • optimization_callback (callable or None) – callback function called during optimization for visualization. If None, automatically gets callback from visualization context.

  • base_limit (float) – absolute bound applied to each base DOF when with_base=True. The same scalar is used for the translational DOFs x, y (in metres) and the rotational DOF theta (in radians), so choose a value large enough to cover the base motion needed by the task in both. Defaults to np.inf (unbounded), which preserves the historical behaviour. Note that scipy’s SLSQP can fail to satisfy the collision constraints when the base is given infinite bounds; in that case passing a finite value (e.g. the reachable base range for the task) makes the line search converge to a collision-free trajectory. Limits coming from continuous joints (e.g. forearm/wrist roll) are always left untouched.

Returns:

planned_trajectory – planned trajectory.

Return type:

numpy.ndarray(n_wp, n_dof)