def update_input(self): m2mm = np.matrix(np.diag(np.array([1000, 1000, 1000, 1, 1, 1]))) mm2m = np.matrix(np.diag(np.array([0.001, 0.001, 0.001, 1, 1, 1]))) jacobi = get_jacobi_matrix(self._robot_state_cur.arm_joint_angles, euler_from_rosquaternion(self._robot_state_cur.base_pose.orientation)[0]) # pinv_jacobi = np.linalg.pinv(jacobi) pinv_jacobi = jacobi.T.dot(np.linalg.inv(jacobi.dot(jacobi.T))) # vec_redundancy = get_vec_redundancy(self._robot_state_cur, jacobi) Kpgain = np.matrix(np.diag(np.array([2, 2, 2, 0, 0, 0]))) Kredundancy = np.matrix(np.diag(np.array([1, 1, 1, 1, 1, 1, 1, 1]))) # rospy.loginfo('\n' + '\n'.join(['EEStateErr:\t' + ', '.join([str(d[0]) for d in self._ee_state_err.tolist()]), # 'EEVelRef:\t' + ', '.join([str(d[0]) for d in self._ee_vel_ref.tolist()])])) u = pinv_jacobi * m2mm * (self._ee_vel_ref - Kpgain * self._ee_state_err) # + (np.identity(8) - pinv_jacobi * jacobi) * Kredundancy * vec_redundancy u = u.round(decimals=self._decimals) v = (u[6, 0] + u[7, 0]) * self._param.Rw / 2 w = (u[6, 0] - u[7, 0]) * self._param.Rw / self._param.T self._ee_state_err_container.write(self._ee_state_err.T.tolist()[0]) self._ee_state_ref_container.write(self._ee_state_ref.T.tolist()[0]) self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0]) self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0]) self._robot_vel_input_container.write(u.T.tolist()[0][:6] + [v, w]) # FIXME u[0, 0] return [JointVelocity(joint1=-u[0, 0], joint2=u[1, 0], joint3=u[2, 0], joint4=u[3, 0], joint5=u[4, 0], joint6=u[5, 0]), TwistStamped(header=Header(stamp=rospy.Time.now()), twist=twist_from_vw(v, w))]
def update_input(self): v, w = self._calc_pd_input() return TwistStamped(header=Header(stamp=rospy.Time.now()), twist=twist_from_vw(v, w))