def from_dict(obj_dict):
obj = Object3d()
obj.id = obj_dict['id']
obj.type = obj_dict['type']
obj.pose = Pose3.from_dict(obj_dict['pose'])
obj.bbox = BBox3.from_dict(obj_dict['bounding_box'])
return obj
def extract_predicate_values(self, data_point_count: int,
goal_positions: np.ndarray,
goal_orientations: np.ndarray,
handle_bb_mins: np.ndarray,
handle_bb_maxs: np.ndarray) -> np.ndarray:
predicate_values = np.zeros(
(data_point_count,
MetaPredicateData.get_predicate_count(GraspingPredicateLibrary)),
dtype=int)
predicates = MetaPredicateData.get_predicates(GraspingPredicateLibrary)
for i in range(data_point_count):
gripper_position = Vector3(x=goal_positions[i, 0],
y=goal_positions[i, 1],
z=goal_positions[i, 2])
gripper_orientation = Vector3(x=goal_orientations[i, 0],
y=goal_orientations[i, 1],
z=goal_orientations[i, 2])
gripper_pose = Pose3(position=gripper_position,
orientation=gripper_orientation)
bb_min = Vector3(x=handle_bb_mins[i, 0],
y=handle_bb_mins[i, 1],
z=handle_bb_mins[i, 2])
bb_max = Vector3(x=handle_bb_maxs[i, 0],
y=handle_bb_maxs[i, 1],
z=handle_bb_maxs[i, 2])
handle_bbox = BBox3(min_values=bb_min, max_values=bb_max)
for j, p in enumerate(predicates):
predicate_values[i, j] = p(gripper_pose, handle_bbox)
return predicate_values
def convert_ros_msg(msg):
'''Converts an 'mcr_perception_msgs.msg.BoundingBox' message to an
'action_execution.geometry.bbox.BBox3' object.
Keyword argumens:
msg -- an 'mcr_perception_msgs.msg.BoundingBox' message
'''
bbox = BBox3()
min_vec = Vector3(msg.center.x - (msg.dimensions.x / 2.),
msg.center.y - (msg.dimensions.y / 2.),
msg.center.z - (msg.dimensions.z / 2.))
max_vec = Vector3(msg.center.x + (msg.dimensions.x / 2.),
msg.center.y + (msg.dimensions.y / 2.),
msg.center.z + (msg.dimensions.z / 2.))
bbox.min = min_vec
bbox.max = max_vec
return bbox
def from_dict(obj_dict):
obj = Plane()
obj.id = obj_dict['id']
obj.pose = Pose3.from_dict(obj_dict['pose'])
obj.bbox = BBox3.from_dict(obj_dict['bounding_box'])
return obj
def __init__(self, plane_id='', pose=Pose3(), bbox=BBox3()):
self.id = plane_id
self.pose = deepcopy(pose)
self.bbox = deepcopy(bbox)
from action_execution.geometry.vector import Vector3
from action_execution.geometry.bbox import BBox3
from action_execution.geometry.pose import Pose3
from action_execution.geometry.object import Object3d
from action_execution.geometry.plane import Plane
from action_execution.utils.configuration import Configuration
obj_config = Configuration()
obj_config.frame_id = 'odom'
static_obj1 = Object3d(obj_type='ketchup_bottle',
pose=Pose3(frame_id='odom',
position=Vector3(-0.75, -0.25, 0.75),
orientation=Vector3(0., 0., 0.)),
bbox=BBox3(Vector3(-0.789, -0.289, 0.75),
Vector3(-0.711, -0.211, 0.958)))
static_obj2 = Object3d(obj_type='ketchup_bottle',
pose=Pose3(frame_id='odom',
position=Vector3(-1., -0.1, 0.75),
orientation=Vector3(0., 0., 0.)),
bbox=BBox3(Vector3(-1.039, -0.139, 0.75),
Vector3(-0.961, -0.061, 0.958)))
static_obj3 = Object3d(obj_type='book',
pose=Pose3(frame_id='odom',
position=Vector3(-1., 0.5, 0.75),
orientation=Vector3(0., 0., 1.57)),
bbox=BBox3(Vector3(-1.225, 0.635, 0.75),
Vector3(-0.775, 0.365, 0.798)))
obj_config.static_objs = [static_obj1, static_obj2, static_obj3]
'''
from action_execution.geometry.vector import Vector3
from action_execution.geometry.bbox import BBox3
from action_execution.geometry.pose import Pose3
from action_execution.geometry.object import Object3d
from action_execution.geometry.plane import Plane
from action_execution.utils.configuration import Configuration
obj_config = Configuration()
obj_config.frame_id = 'odom'
static_obj1 = Object3d(pose=Pose3(frame_id='odom',
position=Vector3(-0.75, -0.25, 0.75),
orientation=Vector3(0., 0., 0.)),
bbox=BBox3(Vector3(-0.789, -0.289, 0.75),
Vector3(-0.711, -0.211, 0.958)))
static_obj2 = Object3d(pose=Pose3(frame_id='odom',
position=Vector3(-0.75, 0.05, 0.75),
orientation=Vector3(0., 0., 0.)),
bbox=BBox3(Vector3(-0.789, 0.011, 0.75),
Vector3(-0.711, 0.089, 0.958)))
obj_config.static_objs = [static_obj1, static_obj2]
obj_config.manipulated_obj = Object3d(
pose=Pose3(frame_id='odom',
position=Vector3(-0.126, 0.190, 0.851),
orientation=Vector3(0., 0., 0.)),
bbox=BBox3(Vector3(-0.164, 0.151, 0.849), Vector3(-0.087, 0.229, 1.057)))
obj_config.surface = Plane(pose=Pose3(frame_id='odom',
position=Vector3(-1., 0., 0.75)),
def __init__(self, obj_id='', obj_type='', pose=Pose3(), bbox=BBox3()):
self.id = obj_id
self.type = obj_type
self.pose = deepcopy(pose)
self.bbox = deepcopy(bbox)
z=goal_positions[i,2])
gripper_orientation = Vector3(x=goal_orientations[i,0],
y=goal_orientations[i,1],
z=goal_orientations[i,2])
gripper_pose = Pose3(position=gripper_position,
orientation=gripper_orientation)
bb_min = Vector3(x=handle_bb_mins[i,0],
y=handle_bb_mins[i,1],
z=handle_bb_mins[i,2])
bb_max = Vector3(x=handle_bb_maxs[i,0],
y=handle_bb_maxs[i,1],
z=handle_bb_maxs[i,2])
handle_bbox = BBox3(min_values=bb_min,
max_values=bb_max)
diagnoses, experience_correction = execution_model.correct_failed_experience(predicate_library=GraspingPredicateLibrary,
state_update_fn=state_update_fn,
failure_search_params=failure_search_params,
diagnosis_repetition_count=50,
diagnosis_candidate_confidence=0.8,
correction_sample_count=10,
pose=gripper_pose,
b_box=handle_bbox)
alternative_experience = action_parameters[i] + experience_correction
if not np.allclose(experience_correction, np.zeros_like(experience_correction)):
original_goal_positions.append(action_parameters[i])
alternative_experiences.append(alternative_experience)
print('Sample: {0}'.format(i+1))