def __init__(self, cfg_file="cfg/config_panda.yaml"): """ Parameters ---------- cfg : dict Dictionary of configuration parameters. """ self._camera_data = CameraData() # parse cfg yaml file with open(cfg_file) as f: self.cfg = yaml.load(f, Loader=yaml.FullLoader) super(SuperquadricsGraspPlanner, self).__init__(self.cfg) self._robot = self.cfg['robot']['name'] if self._robot[-3:] == 'Sim' or self._robot[-3:] == 'sim': pass # TODO something self._icub_base_T_robot_base = np.array( self.cfg['robot']['icub_base_T_robot_base']) self._icub_hand_T_robot_hand = np.array( self.cfg['robot']['icub_hand_T_robot_hand']) self._pointcloud = sb.PointCloud() self._superqs = sb.vector_superquadric() self._grasp_res = sb.GraspResults() self._sq_estimator = sb.SuperqEstimatorApp() self._grasp_estimator = sb.GraspEstimatorApp() self.configure(self.cfg)
def superqs(self): sq_out = sb.vector_superquadric(np.size(self._superqs, 0)) for i, sq in enumerate(self._superqs): sq_out[i] = sq # Express the sq pose wrt the robot base, instead of the icub base quat_sq = tr.axis_angle_to_quaternion( (sq.axisangle[0][0], sq.axisangle[1][0], sq.axisangle[2][0], sq.axisangle[3][0])) pos_sq = [sq.center[0][0], sq.center[1][0], sq.center[2][0]] robot_base_T_icub_base = np.linalg.inv( self._icub_base_T_robot_base) icub_base_T_icub_sq = np.eye(4) icub_base_R_icub_sq = tr.quaternion_to_matrix(quat_sq) icub_base_T_icub_sq[:3] = np.append(icub_base_R_icub_sq, np.array([pos_sq]).T, axis=1) robot_base_T_icub_sq = np.matmul(robot_base_T_icub_base, icub_base_T_icub_sq) vec_aa_sq = tr.quaternion_to_axis_angle( tr.matrix_to_quaternion(robot_base_T_icub_sq[:3, :3])) sq_out[i].setSuperqOrientation(vec_aa_sq) sq_out[i].setSuperqCenter(robot_base_T_icub_sq[:3, 3]) return sq_out
def reset(self): self.grasp_poses = [] self._best_grasp = None self._camera_data = CameraData() self._pointcloud.deletePoints() self._superqs = sb.vector_superquadric() self._grasp_res = sb.GraspResults()
def _compute_superquadric(self, camera_data): # Check point cloud validity if camera_data.pc_img.getNumberPoints() < self.cfg['sq_model']['minimum_points']: print("the point cloud is unvalid. Only {} points." .format(camera_data.pc_img.getNumberPoints())) return False # Compute superquadrics self._superqs = sb.vector_superquadric(self._sq_estimator.computeSuperq(camera_data.pc_img)) return np.size(self._superqs, 0) >= 0