def quaternion_pd(quat_des, quat_cur, omega_des=np.zeros(3), omega_cur=np.zeros(3), omega_dot_des=np.zeros(3), kp=100,
kd=0.0):
r"""
Return the commanded spatial angular acceleration using the orientation PD feedback law given by:
.. math:: \dot{\omega}_c = \dot{\omega}_d + K_D (\omega_d - \omega) + K_P e_o
where :math:`\dot{\omega}_c` is the commanded angular acceleration, \dot{\omega}_d is the desired angular
acceleration, :math:`K_D` is the derivative gain, :math:`\omega` is the angular velocity, :math:`K_P` is the
proportional gain, and :math:`e_o` is the orientation error (which depends on the orientation representation we
are using).
If quaternions are given, the orientation error is given by:
.. math:: e_o = s v_d - s_d v - v_d \cross v
where a quaternion is represented as an ordered pair :math:`[s, v]` with :math:`s \in \mathbb{R}` (the scalar part)
and :math:`v \in \mathbb{R}^3` (the vector part).
Args:
quat_des (np.array[4]): desired quaternion [x,y,z,w]
quat_cur (np.array[4]): current quaternion [x,y,z,w]
omega_des (np.array[3]): desired angular velocity
omega_cur (np.array[3]): current angular velocity
omega_dot_des (np.array[3]): desired angular acceleration
kp (float, np.array[3,3]): proportional gain
kd (float, np.array[3,3]): derivative gain
Returns:
np.array[3]: the commanded spatial angular acceleration computed by the orientation PD feedback law
"""
return kp * quaternion_error(quat_des=quat_des, quat_cur=quat_cur) + kd * (omega_des - omega_cur) + omega_dot_des
def _update(self, x=None):
"""
Update the task by computing the A matrix and b vector that will be used by the task solver.
"""
# get useful variables
x = self.model.get_pose(link=self.distal_link,
wrt_link=self.base_link) # (7,)
dx = self.model.get_velocity(link=self.distal_link,
wrt_link=self.base_link) # (6,)
jac = self.model.get_jacobian(
link=self.distal_link,
wrt_link=self.base_link,
point=self.local_position) # shape: (6,N)
H = self.model.get_inertia_matrix() # shape: (N,N)
H_inv = np.linalg.inv(H)
if self._des_quat is None: # only position and/or velocities
if self._des_pos is None: # only velocities
force = np.concatenate(
(np.dot(self.kd_linear, (self._des_lin_vel - dx[:3])),
np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))))
else: # only position
jac = jac[:3]
position = np.dot(self.kp_position, (self._des_pos - x[:3]))
lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - dx[:3]))
force = position + lin_vel
elif self._des_pos is None: # only orientation
jac = jac[3:]
orientation = np.dot(
self.kp_orientation,
quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]))
ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))
force = orientation + ang_vel
else: # both
# compute position/orientation error
position = np.dot(self.kp_position, (self._des_pos - x[:3]))
orientation = np.dot(
self.kp_orientation,
quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]))
# compute velocities
lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - dx[:3]))
ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))
force = np.concatenate((position + lin_vel, orientation + ang_vel))
# compute A matrix and b vector
self._A = jac.dot(H_inv)
self._b = self._A.dot(jac.T.dot(force))
def _update(self, x=None):
"""
Update the task by computing the A matrix and b vector that will be used by the task solver.
"""
x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)
self._A = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
point=self.local_position) # shape: (6,N)
vel = self.model.get_velocity(link=self.distal_link, wrt_link=self.base_link)
jdotqdot = self.model.compute_JdotQdot(link=self.distal_link)
# b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)
b = -jdotqdot + self.desired_acceleration
if self._des_quat is None: # only position and/or velocities
if self._des_pos is None: # only velocities
self._b = b + np.concatenate((np.dot(self.kd_linear, (self._des_lin_vel - vel[:3])),
np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))))
else: # only position
self._A = self._A[:3]
# compute position error
error = (self._des_pos - x[:3])
# compute b vector
lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3]))
self._b = b[:3] + np.dot(self.kp_position, error) + lin_vel
elif self._des_pos is None: # only orientation
self._A = self._A[3:]
# compute orientation error
error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:])
# compute b vector
ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))
self._b = b[3:] + np.dot(self.kp_orientation, error) + ang_vel
else: # both
# compute position/orientation error
position_error = (self._des_pos - x[:3])
orientation_error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:])
# compute b vector
lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3]))
ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))
b_lin = np.dot(self.kp_position, position_error) + lin_vel
b_ang = np.dot(self.kp_orientation, orientation_error) + ang_vel
self._b = b + np.concatenate((b_lin, b_ang))
def _update(self, x=None):
"""
Update the task by computing the A matrix and b vector that will be used by the task solver.
"""
x = self.model.get_pose(self.distal_link, self.base_link)
self._A = self.model.get_jacobian(
link=self.distal_link,
wrt_link=self.base_link,
point=self.local_position) # shape: (6,N)
if self._des_quat is None: # only position and/or velocities
if self._des_pos is None: # only velocities
self._b = np.concatenate(
(self._des_lin_vel, self._des_ang_vel))
else: # only position
self._A = self._A[:3]
# compute position error
error = (self._des_pos - x[:3])
# compute b vector
self._b = np.dot(self.kp_position, error) + self._des_lin_vel
elif self._des_pos is None: # only orientation
self._A = self._A[3:]
# compute orientation error
error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:])
# compute b vector
self._b = np.dot(self.kp_orientation, error) + self._des_ang_vel
else: # both
# compute position/orientation error
position_error = (self._des_pos - x[:3])
orientation_error = quaternion_error(quat_des=self._des_quat,
quat_cur=x[3:])
# compute b vector
b_position = np.dot(self.kp_position,
position_error) + self._des_lin_vel
b_orientation = np.dot(self.kp_orientation,
orientation_error) + self._des_ang_vel
self._b = np.concatenate((b_position, b_orientation))
