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)[source]

Gradient based trajectory optimization using scipy’s SLSQP.

Collision constraint is considered in an inequality constraits. 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 specifeid 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.

Returns:

planned_trajectory – planned trajectory.

Return type:

numpy.ndarray(n_wp, n_dof)