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