ctrlutils.pd_control

ctrlutils.pd_control(*args, **kwargs)

Overloaded function.

1. pd_control(x: numpy.ndarray[numpy.float64[m, 1]], x_des: numpy.ndarray[numpy.float64[m, 1]], dx: numpy.ndarray[numpy.float64[m, 1]], kp_kv: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], ddx_max: float = 0.0) -> Tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]

   General PD control law for reaching a goal position with velocity damping.

   Outputs a desired acceleration given a desired position, as well as the current position and velocity. This behaves like a damped spring-mass system.

   Control law:

   ddx = -kp (x - x_des) - kv dx

   Args:

   x: Current position as an N-D vector. x_des: Desired position as an N-D vector. dx: Current velocity as an N-D vector. kp_kv: Gains matrix as a 2D vector [kp, kv] or an N x 2 matrix, where

   the first and second columns correspond to kp and kv, respectively, and the N rows represent per-dimension gains.

   ddx_max: Optional maximum acceleration due to the position error. This

   value clips the acceleration when the distance to the goal is large and is ignored when less than or equal to 0.

   Returns:

   2-tuple (ddx, x_err).

2. pd_control(quat: Eigen::Quaternion<double, 0>, quat_des: Eigen::Quaternion<double, 0>, w: numpy.ndarray[numpy.float64[3, 1]], kp_kv: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], dw_max: float = 0.0) -> Tuple[numpy.ndarray[numpy.float64[3, 1]], numpy.ndarray[numpy.float64[3, 1]]]

   Special PD control law for 3D orientations.

   Outputs a desired acceleration given a desired orientation, as well as the current orientation and angular velocity. This behaves like a damped spring-mass system.

   Control law:

   dw = -kp (quat - quat_des) - kv w

   Args:

   quat: Current orientation as a quaternion. quat_des: Desired orientation as a quaternion. w: Current angular velocity as a 4D vector. kp_kv: Gains matrix as a 2D vector [kp, kv] or a 3 x 2 matrix, where

   the first and second columns correspond to kp and kv, respectively, and the 3 rows represent per-dimension (x, y, z) gains.

   dw_max: Optional maximum acceleration due to the orientation error. This

   value clips the acceleration when the distance to the goal is large and is ignored when less than or equal to 0.

   Returns:

   2-tuple (dw, w_err).