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))