def parse_robot(robot_json):
pose = parse_pose(robot_json)
if robot_json['name'] == 'pr2':
with HideOutput(True):
robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_group_conf(robot_id, 'base', base_values_from_pose(pose))
else:
# TODO: set the z?
#set_pose(robot_id, pose)
raise NotImplementedError(robot_json['name'])
for joint, values in robot_json['conf'].items():
[value] = values
if has_joint(robot_id, joint):
set_joint_position(robot_id, joint_from_name(robot_id, joint),
value)
else:
print('Robot {} lacks joint {}'.format(robot_json['name'], joint))
if robot_json['name'] == 'pr2':
set_group_conf(robot_id, 'torso', [0.2])
set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM)
set_group_conf(robot_id, 'right_arm',
rightarm_from_leftarm(REST_LEFT_ARM))
return robot_id
def fn(pose, surface):
# P(obs point | state detect)
if surface is None:
return DeltaDist(None)
# Weight can be proportional weight in the event that the distribution can't be normalized
x, y, yaw = base_values_from_pose(pose.get_reference_from_body())
if DIM == 2:
return ProductDistribution([
GaussianDistribution(gmean=x, stdev=MODEL_POS_STD),
GaussianDistribution(gmean=y, stdev=MODEL_POS_STD),
#CUniformDist(-np.pi, +np.pi),
])
else:
return SE2Distribution(x,
y,
yaw,
pos_std=MODEL_POS_STD,
ori_std=MODEL_ORI_STD)
def parse_robot(robot):
name = robot.find('name').text
urdf = robot.find('urdf').text
fixed_base = not parse_boolean(robot.find('movebase'))
print(name, urdf, fixed_base)
pose = parse_pose(robot.find('basepose'))
torso = parse_array(robot.find('torso'))
left_arm = parse_array(robot.find('left_arm'))
right_arm = parse_array(robot.find('right_arm'))
assert (name == 'pr2')
with HideOutput():
robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_group_conf(robot_id, 'base', base_values_from_pose(pose))
set_group_conf(robot_id, 'torso', torso)
set_group_conf(robot_id, 'left_arm', left_arm)
set_group_conf(robot_id, 'right_arm', right_arm)
#set_point(robot_id, Point(z=point_from_pose(pose)[2]))
# TODO: base pose isn't right
# print(robot.tag)
# print(robot.attrib)
# print(list(robot.iter('basepose')))
return robot_id
def pose2d_from_pose(self, pose):
return base_values_from_pose(pose.get_reference_from_body())[:DIM]