def generic_setup(km, robot_path, eef_link='gripper_link'):
joint_symbols = [j.position for j in km.get_data(f'{robot_path}/joints').values()
if hasattr(j, 'position') and gm.is_symbol(j.position)]
robot_controlled_symbols = {gm.DiffSymbol(j) for j in joint_symbols} # if 'torso' not in str(j)}
blacklist = set()
return joint_symbols, \
robot_controlled_symbols, \
{}, \
km.get_data(robot_path).links[eef_link], \
blacklist
def _execute_impl(self, door_position, handle_position, locking_tolerance,
handle_release_angle):
door_vel = gm.DiffSymbol(door_position)
is_unlocked = gm.alg_not(
gm.alg_and(gm.less_than(door_position, locking_tolerance),
gm.less_than(handle_position, handle_release_angle)))
self.constraints = {
f'lock {door_position}': Constraint(-1e9, is_unlocked * 1e9,
door_vel)
}
def __init__(self, km, controlled_symbols, resting_pose, camera_path=None):
tucking_constraints = {}
if resting_pose is not None:
tucking_constraints = {
f'tuck {s}': SC(p - s, p - s, 1, s)
for s, p in resting_pose.items()
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
# tucking_constraints.update(self.taxi_constraints)
self.use_camera = camera_path is not None
if camera_path is not None:
self._poi_pos = gm.Symbol('poi')
poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3(
0, self._poi_pos * 2.0, 0)
camera = km.get_data(camera_path)
cam_to_poi = poi - gm.pos_of(camera.pose)
lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose),
cam_to_poi) / gm.norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= gm.free_symbols(c.expr)
joint_symbols = {
s
for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN
}
controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols}
hard_constraints = km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
km, controlled_values)
self.qp = TQPB(hard_constraints, tucking_constraints,
controlled_values)
self._start = Time.now()
def pr2_setup(km, pr2_path):
start_pose = {'l_elbow_flex_joint' : -2.1213,
'l_shoulder_lift_joint': 1.2963,
'l_wrist_flex_joint' : -1.16,
'l_shoulder_pan_joint': 0.7,
'r_shoulder_pan_joint': -1.0,
'r_shoulder_lift_joint': 0.9,
'r_upper_arm_roll_joint': -1.2,
'r_elbow_flex_joint' : -2.1213,
'r_wrist_flex_joint' : -1.05,
'r_forearm_roll_joint': 3.14,
'r_wrist_roll_joint': 0,
'torso_lift_joint' : 0.16825}
start_pose = {gm.Position(Path(f'{pr2_path}/{n}')): v for n, v in start_pose.items()}
joint_symbols = [j.position for j in km.get_data(f'{pr2_path}/joints').values()
if hasattr(j, 'position') and gm.is_symbol(j.position)]
robot_controlled_symbols = {gm.DiffSymbol(j) for j in joint_symbols} # if 'torso' not in str(j)}
blacklist = {gm.Velocity(Path(f'{pr2_path}/torso_lift_joint'))}
return joint_symbols, \
robot_controlled_symbols, \
start_pose, \
km.get_data(pr2_path).links['r_gripper_tool_frame'], \
blacklist
visualizer = ROSBPBVisualizer('~vis', base_frame=reference_frame)
model_name = load_localized_model(km, model_path, reference_frame)
km.clean_structure()
km.dispatch_events()
eef_link = rospy.get_param('~eef_link', 'r_gripper_r_finger_tip_link')
joint_symbols = [
j.position for j in km.get_data(f'pr2/joints').values()
if hasattr(j, 'position') and gm.is_symbol(j.position)
]
robot_controlled_symbols = {
gm.DiffSymbol(j)
for j in joint_symbols if 'torso' not in str(j)
}
base_symbols = None
if use_base:
base_joint = km.get_data(base_joint_path)
base_symbols = BaseSymbols(base_joint.x_pos, base_joint.y_pos,
base_joint.a_pos,
gm.DiffSymbol(base_joint.x_pos),
gm.DiffSymbol(base_joint.y_pos),
gm.DiffSymbol(base_joint.a_pos))
robot_controlled_symbols |= {
gm.DiffSymbol(x)
for x in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos]
}
if robot_name == 'pr2':
joint_symbols, \
robot_controlled_symbols, \
start_pose, \
eef, \
blacklist = pr2_setup(km, robot_path)
elif robot_name == 'fetch':
joint_symbols, \
robot_controlled_symbols, \
start_pose, \
eef, \
blacklist = generic_setup(km, robot_path, rospy.get_param('~eef', 'gripper_link'))
if rospy.get_param('~use_base', False):
base_joint = robot.joints['to_world']
robot_controlled_symbols |= {base_joint.l_wheel_vel, base_joint.r_wheel_vel}
robot_controlled_symbols.difference_update(gm.DiffSymbol(s) for s in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos])
blacklist.update(gm.DiffSymbol(s) for s in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos])
integration_rules = {
base_joint.x_pos: base_joint.x_pos + sym_dt * (base_joint.r_wheel_vel * gm.cos(base_joint.a_pos) * base_joint.wheel_radius * 0.5 + base_joint.l_wheel_vel * gm.cos(base_joint.a_pos) * base_joint.wheel_radius * 0.5),
base_joint.y_pos: base_joint.y_pos + sym_dt * (base_joint.r_wheel_vel * gm.sin(base_joint.a_pos) * base_joint.wheel_radius * 0.5 + base_joint.l_wheel_vel * gm.sin(base_joint.a_pos) * base_joint.wheel_radius * 0.5),
base_joint.a_pos: base_joint.a_pos + sym_dt * (base_joint.r_wheel_vel * (base_joint.wheel_radius / base_joint.wheel_distance) + base_joint.l_wheel_vel * (- base_joint.wheel_radius / base_joint.wheel_distance))}
start_pose = {'wrist_roll_joint' : 0.0,
'shoulder_pan_joint' : 0.3,
'elbow_flex_joint' : 1.72,
'forearm_roll_joint' : -1.2,
'upperarm_roll_joint': -1.57,
'wrist_flex_joint' : 1.66,
'shoulder_lift_joint': 1.4,
'torso_lift_joint' : 0.2}
start_pose = {gm.Position(Path(f'{robot_path}/{n}')): v for n, v in start_pose.items()}
else:
def behavior_update(self):
state_count = 0
while not rospy.is_shutdown() and not self._kys:
loop_start = rospy.Time.now()
with self._state_lock:
if self._robot_state_update_count <= state_count:
rospy.sleep(0.01)
continue
state_count = self._robot_state_update_count
if self.controller is not None:
now = rospy.Time.now()
with self._state_lock:
deltaT = 0.05 if self._last_controller_update is None else (
now - self._last_controller_update).to_sec()
try:
command = self.controller.get_cmd(self._state,
deltaT=deltaT)
except Exception as e:
print(traceback.format_exc())
rospy.signal_shutdown('die lol')
self.robot_command_processor.send_command(command)
self._last_controller_update = now
# Lets not confuse the tracker
if self._phase != 'opening' and self._last_external_cmd_msg is not None:
# Ensure that velocities are assumed to be 0 when not operating anything
self._last_external_cmd_msg.value = [0] * len(
self._last_external_cmd_msg.value)
self.pub_external_command.publish(self._last_external_cmd_msg)
self._last_external_cmd_msg = None
if self._phase == 'idle':
# if there is no controller, instantiate idle
# check for new target
# -> open gripper
# instantiate 6d controller
# switch to "grasping"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
with self._state_lock:
for s, p in self._target_map.items():
if s in self._state and self._state[
s] < self._var_upper_bound[
s] * self.monitoring_threshold: # Some thing in the scene is closed
self._current_target = p
print(f'New target is {self._current_target}')
self.gripper_wrapper.sync_set_gripper_position(
0.07)
self._last_controller_update = None
print('Gripper is open. Proceeding to grasp...')
draw_fn = None
eef_pose = self.km.get_data(self.eef_path).pose
if self.visualizer is not None:
self.visualizer.begin_draw_cycle('grasp pose')
self.visualizer.draw_poses(
'grasp pose', np.eye(4), 0.2, 0.01, [
gm.subs(self._grasp_poses[p],
self._state)
])
self.visualizer.render('grasp pose')
def draw_fn(state):
self.visualizer.begin_draw_cycle(
'debug_grasp')
self.visualizer.draw_poses(
'debug_grasp', np.eye(4), 1.0, 0.01, [
gm.subs(eef_pose, state),
gm.subs(self._grasp_poses[p],
state)
])
self.visualizer.render('debug_grasp')
# print('\n'.join(f'{s}: {v}' for s, v in state.items()))
self.controller = SixDPoseController(
self.km, eef_pose, self._grasp_poses[p],
self.controlled_symbols, self.weights, draw_fn)
self._phase = 'grasping'
print(f'Now entering {self._phase} state')
break
else:
print(
'None of the monitored symbols are closed:\n {}'.
format('\n '.join(
f'{self._state[s]} < {self._var_upper_bound[s]}'
for s in self._target_map.keys()
if s in self._state)))
elif self._phase == 'grasping':
# check if grasp is acheived
# -> close gripper
# instantiate cascading controller
# switch to "opening"
# if there is no more command but the goal error is too great -> "homing"
if self.controller.equilibrium_reached(0.03, -0.03):
if self.controller.current_error() > 0.01:
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
else:
print('Closing gripper...')
self.gripper_wrapper.sync_set_gripper_position(0, 80)
print('Generating opening controller')
eef = self.km.get_data(self.eef_path)
obj = self.km.get_data(self._current_target)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
static_object_pose = gm.subs(obj.pose, self._state)
offset_pose = np_inverse_frame(static_object_pose).dot(
static_eef_pose)
print(offset_pose)
goal_pose = gm.dot(obj.pose, gm.Matrix(offset_pose))
goal_pos_error = gm.norm(
gm.pos_of(eef.pose) - gm.pos_of(goal_pose))
goal_rot_error = gm.norm(eef.pose[:3, :3] -
goal_pose[:3, :3])
target_symbols = self._target_body_map[
self._current_target]
lead_goal_constraints = {
f'open_{s}': SC(self._var_upper_bound[s] - s, 1000,
1, s)
for s in target_symbols
if s in self._var_upper_bound
}
for n, c in lead_goal_constraints.items():
print(f'{n}: {c}')
follower_goal_constraints = {
'keep position':
SC(-goal_pos_error, -goal_pos_error, 10,
goal_pos_error),
'keep rotation':
SC(-goal_rot_error, -goal_rot_error, 1,
goal_rot_error)
}
blacklist = {
gm.Velocity(Path('pr2/torso_lift_joint'))
}.union({
gm.DiffSymbol(s)
for s in gm.free_symbols(obj.pose)
if 'location' in str(s)
})
# blacklist = {gm.DiffSymbol(s) for s in gm.free_symbols(obj.pose) if 'location' in str(s)}
self.controller = CascadingQP(
self.km,
lead_goal_constraints,
follower_goal_constraints,
f_gen_follower_cvs=gen_dv_cvs,
controls_blacklist=blacklist,
t_follower=GQPB,
visualizer=None # self.visualizer
)
print(self.controller)
# def debug_draw(vis, state, cmd):
# vis.begin_draw_cycle('lol')
# vis.draw_poses('lol', np.eye(4), 0.2, 0.01,
# [gm.subs(goal_pose, state),
# gm.subs(eef.pose, state)])
# vis.render('lol')
# self.controller.follower_qp._cb_draw = debug_draw
# self.gripper_wrapper.sync_set_gripper_position(0.07)
# rospy.signal_shutdown('die lol')
# return
self._last_controller_update = None
self._phase = 'opening'
print(f'Now entering {self._phase} state')
elif self._phase == 'opening':
# Wait for monitored symbols to be in open position
# -> open gripper
# generate 6d retraction goal: -10cm in tool frame
# spawn 6d controller
# switch to "retracting"
# self.visualizer.begin_draw_cycle('world state')
# with self._state_lock:
# self._world.update_world(self._state)
# self.visualizer.draw_world('world state', self._world, b=0)
# self.visualizer.render('world state')
external_command = {
s: v
for s, v in command.items()
if s not in self.controlled_symbols
}
ext_msg = ValueMapMsg()
ext_msg.header.stamp = now
ext_msg.symbol, ext_msg.value = zip(
*[(str(s), v) for s, v in external_command.items()])
self.pub_external_command.publish(ext_msg)
self._last_external_cmd_msg = ext_msg
with self._state_lock:
current_external_error = {
s: self._state[s]
for s in self._target_body_map[self._current_target]
}
print('Current error state:\n {}'.format('\n '.join(
[f'{s}: {v}' for s, v in current_external_error.items()])))
gripper_err = self.gripper_wrapper.get_latest_error()
# if gripper_err < 0.0005: # Grasped object is thinner than 5mm aka we lost it
# self.controller = self._idle_controller
# self._phase = 'homing'
# print(f'Grasped object seems to have slipped ({gripper_err}). Returning to home...')
# return
if min([
v >=
self._var_upper_bound[s] * self.acceptance_threshold
for s, v in current_external_error.items()
]):
print('Target fulfilled. Setting it to None')
self._current_target = None
eef = self.km.get_data(self.eef_path)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
goal_pose = static_eef_pose.dot(
np.array([[1, 0, 0, -0.12], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
self.controller = SixDPoseController(
self.km, eef.pose, goal_pose, self.controlled_symbols,
self.weights)
self.gripper_wrapper.sync_set_gripper_position(0.07)
self._last_controller_update = None
self._phase = 'retracting'
print(f'Now entering {self._phase} state')
else:
remainder = (1 / 3) - (rospy.Time.now() -
loop_start).to_sec()
if remainder > 0:
rospy.sleep(remainder)
elif self._phase == 'retracting':
# Wait for retraction to complete (Currently just skipping)
# -> spawn idle controller
# switch to "homing"
if self.controller.equilibrium_reached(0.035, -0.035):
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
elif self._phase == 'homing':
# Wait for idle controller to have somewhat low error
# -> switch to "idle"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
continue
if self.controller.equilibrium_reached(0.1, -0.1):
self._phase = 'idle'
print(f'Now entering {self._phase} state')
else:
raise Exception(f'Unknown state "{self._phase}')
def create_nobilia_shelf(km,
prefix,
origin_pose=gm.eye(4),
parent_path=Path('world')):
km.apply_operation(
f'create {prefix}',
ExecFunction(prefix, MarkedArticulatedObject, str(prefix)))
shelf_height = 0.72
shelf_width = 0.6
shelf_body_depth = 0.35
wall_width = 0.016
l_prefix = prefix + ('links', )
geom_body_wall_l = Box(
l_prefix + ('body', ),
gm.translation3(0, 0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_wall_r = Box(
l_prefix + ('body', ),
gm.translation3(0, -0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_ceiling = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, 0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_floor = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, -0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_shelf_1 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, -0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_shelf_2 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, 0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_back = Box(
l_prefix + ('body', ),
gm.translation3(0.5 * (shelf_body_depth - 0.005), 0, 0),
gm.vector3(0.005, shelf_width - 2 * wall_width,
shelf_height - 2 * wall_width))
shelf_geom = [
geom_body_wall_l, geom_body_wall_r, geom_body_ceiling, geom_body_floor,
geom_body_back, geom_body_shelf_1, geom_body_shelf_2
]
rb_body = RigidBody(parent_path,
origin_pose,
geometry=dict(enumerate(shelf_geom)),
collision=dict(enumerate(shelf_geom)))
geom_panel_top = Box(l_prefix + ('panel_top', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
geom_panel_bottom = Box(l_prefix + ('panel_bottom', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
handle_width = 0.16
handle_depth = 0.05
handle_diameter = 0.012
geom_handle_r = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_l = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
-0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_bar = Box(
l_prefix + ('handle', ),
gm.translation3(handle_depth - 0.5 * handle_diameter, 0, 0),
gm.vector3(handle_diameter, handle_width - handle_diameter,
handle_diameter))
handle_geom = [geom_handle_l, geom_handle_r, geom_handle_bar]
# Sketch of mechanism
#
# T ---- a
# ---- \ Z
# b ..... V \
# | ...... d
# B | ------
# c ------
# L
#
# Diagonal V is virtual
#
#
# Angles:
# a -> alpha (given)
# b -> gamma_1 + gamma_2 = gamma
# c -> don't care
# d -> delta_1 + delta_2 = delta
#
opening_position = gm.Position(prefix + ('door', ))
# Calibration results
#
# Solution top hinge: cost = 0.03709980624159568 [ 0.08762252 -0.01433833 0.2858676 0.00871125]
# Solution bottom hinge: cost = 0.025004236048128934 [ 0.1072496 -0.01232362 0.27271013 0.00489996]
# Added 180 deg rotation due to -x being the forward facing side in this model
top_hinge_in_body_marker = gm.translation3(0.08762252 - 0.015, 0,
-0.01433833)
top_panel_marker_in_top_hinge = gm.translation3(0.2858676 - 0.003,
-wall_width + 0.0025,
0.00871125 - 0.003)
front_hinge_in_top_panel_maker = gm.translation3(0.1072496 - 0.02, 0,
-0.01232362 + 0.007)
bottom_panel_marker_in_front_hinge = gm.translation3(
0.27271013, 0, 0.00489996)
# Top hinge - Data taken from observation
body_marker_in_body = gm.dot(
gm.rotation3_axis_angle(gm.vector3(0, 0, 1), math.pi),
gm.translation3(0.5 * shelf_body_depth - 0.062,
-0.5 * shelf_width + 0.078, 0.5 * shelf_height))
top_panel_marker_in_top_panel = gm.translation3(
geom_panel_top.scale[0] * 0.5 - 0.062,
-geom_panel_top.scale[1] * 0.5 + 0.062, geom_panel_top.scale[2] * 0.5)
bottom_panel_marker_in_bottom_panel = gm.translation3(
geom_panel_bottom.scale[0] * 0.5 - 0.062,
-geom_panel_bottom.scale[1] * 0.5 + 0.062,
geom_panel_bottom.scale[2] * 0.5)
top_hinge_in_body = gm.dot(body_marker_in_body, top_hinge_in_body_marker)
top_panel_in_top_hinge = gm.dot(
top_panel_marker_in_top_hinge,
gm.inverse_frame(top_panel_marker_in_top_panel))
front_hinge_in_top_panel = gm.dot(top_panel_marker_in_top_panel,
front_hinge_in_top_panel_maker)
bottom_panel_in_front_hinge = gm.dot(
bottom_panel_marker_in_front_hinge,
gm.inverse_frame(bottom_panel_marker_in_bottom_panel))
# Point a in body reference frame
point_a = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(top_hinge_in_body))
point_d = gm.point3(-shelf_body_depth * 0.5 + 0.09, 0,
shelf_height * 0.5 - 0.192)
# point_d = gm.point3(-shelf_body_depth * 0.5 + gm.Symbol('point_d_x'), 0, shelf_height * 0.5 - gm.Symbol('point_d_z'))
# Zero alpha along the vertical axis
vec_a_to_d = gm.dot(point_d - point_a)
alpha = gm.atan2(vec_a_to_d[0], -vec_a_to_d[2]) + opening_position
top_panel_in_body = gm.dot(
top_hinge_in_body, # Translation hinge to body frame
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position +
0.5 * math.pi), # Hinge around y
top_panel_in_top_hinge)
front_hinge_in_body = gm.dot(top_panel_in_body, front_hinge_in_top_panel)
# Point b in top panel reference frame
point_b_in_top_hinge = gm.pos_of(
gm.dot(gm.diag(1, 0, 1, 1), front_hinge_in_top_panel,
top_panel_in_top_hinge))
point_b = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(front_hinge_in_body))
# Hinge lift arm in body reference frame
point_c_in_bottom_panel = gm.dot(
gm.diag(1, 0, 1, 1),
bottom_panel_marker_in_bottom_panel,
gm.point3(-0.094, -0.034, -0.072),
# gm.point3(-gm.Symbol('point_c_x'), -0.034, -gm.Symbol('point_c_z'))
)
point_c_in_front_hinge = gm.dot(
gm.diag(1, 0, 1, 1),
gm.dot(bottom_panel_in_front_hinge, point_c_in_bottom_panel))
length_z = gm.norm(point_a - point_d)
vec_a_to_b = point_b - point_a
length_t = gm.norm(vec_a_to_b)
length_b = gm.norm(point_c_in_front_hinge[:3])
# length_l = gm.Symbol('length_l') # 0.34
length_l = 0.372
vec_b_to_d = point_d - point_b
length_v = gm.norm(vec_b_to_d)
gamma_1 = inner_triangle_angle(length_t, length_v, length_z)
gamma_2 = inner_triangle_angle(length_b, length_v, length_l)
top_panel_offset_angle = gm.atan2(point_b_in_top_hinge[2],
point_b_in_top_hinge[0])
bottom_offset_angle = gm.atan2(point_c_in_front_hinge[2],
point_c_in_front_hinge[0])
gamma = gamma_1 + gamma_2
rb_panel_top = RigidBody(l_prefix + ('body', ),
gm.dot(rb_body.pose, top_panel_in_body),
top_panel_in_body,
geometry={0: geom_panel_top},
collision={0: geom_panel_top})
# old offset: 0.5 * geom_panel_top.scale[2] + 0.03
tf_bottom_panel = gm.dot(
front_hinge_in_top_panel,
gm.rotation3_axis_angle(
gm.vector3(0, 1, 0),
math.pi + bottom_offset_angle - top_panel_offset_angle),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), gamma),
bottom_panel_in_front_hinge)
rb_panel_bottom = RigidBody(l_prefix + ('panel_top', ),
gm.dot(rb_panel_top.pose, tf_bottom_panel),
tf_bottom_panel,
geometry={0: geom_panel_bottom},
collision={0: geom_panel_bottom})
handle_transform = gm.dot(
gm.translation3(geom_panel_bottom.scale[0] * 0.5 - 0.08, 0,
0.5 * wall_width),
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -math.pi * 0.5))
rb_handle = RigidBody(l_prefix + ('panel_bottom', ),
gm.dot(rb_panel_bottom.pose, handle_transform),
handle_transform,
geometry={x: g
for x, g in enumerate(handle_geom)},
collision={x: g
for x, g in enumerate(handle_geom)})
# Only debugging
point_c = gm.dot(rb_panel_bottom.pose, point_c_in_bottom_panel)
vec_b_to_c = point_c - point_b
km.apply_operation(f'create {prefix}/links/body',
CreateValue(rb_panel_top.parent, rb_body))
km.apply_operation(f'create {prefix}/links/panel_top',
CreateValue(rb_panel_bottom.parent, rb_panel_top))
km.apply_operation(
f'create {prefix}/links/panel_bottom',
CreateValue(l_prefix + ('panel_bottom', ), rb_panel_bottom))
km.apply_operation(f'create {prefix}/links/handle',
CreateValue(l_prefix + ('handle', ), rb_handle))
km.apply_operation(
f'create {prefix}/joints/hinge',
ExecFunction(
prefix + Path('joints/hinge'), RevoluteJoint,
CPath(rb_panel_top.parent), CPath(rb_panel_bottom.parent),
opening_position, gm.vector3(0, 1, 0), gm.eye(4), 0, 1.84, **{
f'{opening_position}':
Constraint(0 - opening_position, 1.84 - opening_position,
opening_position),
f'{gm.DiffSymbol(opening_position)}':
Constraint(-0.25, 0.25, gm.DiffSymbol(opening_position))
}))
m_prefix = prefix + ('markers', )
km.apply_operation(
f'create {prefix}/markers/body',
ExecFunction(m_prefix + ('body', ), Frame,
CPath(l_prefix + ('body', )),
gm.dot(rb_body.pose,
body_marker_in_body), body_marker_in_body))
km.apply_operation(
f'create {prefix}/markers/top_panel',
ExecFunction(m_prefix + ('top_panel', ), Frame,
CPath(l_prefix + ('panel_top', )),
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
top_panel_marker_in_top_panel))
km.apply_operation(
f'create {prefix}/markers/bottom_panel',
ExecFunction(
m_prefix + ('bottom_panel', ), Frame,
CPath(l_prefix + ('panel_bottom', )),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel),
bottom_panel_marker_in_bottom_panel))
return NobiliaDebug(
[
top_hinge_in_body,
gm.dot(
top_hinge_in_body,
gm.rotation3_axis_angle(gm.vector3(0, 1, 0),
-opening_position + 0.5 * math.pi),
top_panel_in_top_hinge, front_hinge_in_top_panel),
body_marker_in_body,
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel)
], [(point_a, vec_a_to_d), (point_a, vec_a_to_b),
(point_b, vec_b_to_d), (point_b, vec_b_to_c)],
{
'gamma_1':
gamma_1,
'gamma_1 check_dot':
gamma_1 - gm.acos(
gm.dot_product(-vec_a_to_b / gm.norm(vec_a_to_b),
vec_b_to_d / gm.norm(vec_b_to_d))),
'gamma_1 check_cos':
gamma_1 - inner_triangle_angle(
gm.norm(vec_a_to_b), gm.norm(vec_b_to_d), gm.norm(vec_a_to_d)),
'gamma_2':
gamma_2,
'gamma_2 check_dot':
gamma_2 - gm.acos(
gm.dot_product(vec_b_to_c / gm.norm(vec_b_to_c),
vec_b_to_d / gm.norm(vec_b_to_d))),
'length_v':
length_v,
'length_b':
length_b,
'length_l':
length_l,
'position':
opening_position,
'alpha':
alpha,
'dist c d':
gm.norm(point_d - point_c)
}, {
gm.Symbol('point_c_x'): 0.094,
gm.Symbol('point_c_z'): 0.072,
gm.Symbol('point_d_x'): 0.09,
gm.Symbol('point_d_z'): 0.192,
gm.Symbol('length_l'): 0.372
})
def __init__(self,
km,
actuated_object_path,
target_object_path,
controlled_symbols,
start_state,
camera_path=None,
navigation_method='cross',
visualizer=None,
weight_override=None,
collision_avoidance_paths=None):
print(f'{actuated_object_path}\n{target_object_path}')
actuated_object = km.get_data(actuated_object_path)
target_object = km.get_data(target_object_path)
all_controlled_symbols = controlled_symbols.union({
gm.DiffSymbol(j)
for j in gm.free_symbols(target_object.pose)
if 'location' not in str(j)
})
static_symbols = {
s
for s in gm.free_symbols(target_object.pose)
if 'location' in str(s)
}
# Generate push problem
constraints, \
geom_distance, \
coll_world, \
self.p_internals = generate_push_closing(km,
start_state,
all_controlled_symbols,
actuated_object.pose,
target_object.pose,
actuated_object_path,
target_object_path,
navigation_method,
static_symbols=static_symbols)
start_state.update({s: 0.0 for s in gm.free_symbols(coll_world)})
weight_override = {} if weight_override is None else weight_override
controlled_values, \
constraints = generate_controlled_values(constraints,
all_controlled_symbols)
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.1)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.00)
}
for s, w in weight_override.items():
for cv in controlled_values.values():
if cv.symbol is s:
cv.weight_id = w
break
# CAMERA STUFF
if camera_path is not None:
camera = km.get_data(camera_path)
cam_pos = gm.pos_of(camera.pose)
cam_to_obj = gm.pos_of(target_object.pose) - cam_pos
cam_forward = gm.dot(camera.pose, gm.vector3(1, 0, 0))
look_goal = 1 - (gm.dot_product(cam_to_obj, cam_forward) /
gm.norm(cam_to_obj))
goal_constraints['look_at_obj'] = SC(-look_goal, -look_goal, 1,
look_goal)
# GOAL CONSTAINT GENERATION
# 'avoid_collisions': SC.from_constraint(closest_distance_constraint_world(eef_pose, eef_path[:-1], 0.03), 100)
# }
if collision_avoidance_paths is not None:
for p in collision_avoidance_paths:
obj = km.get_data(p)
goal_constraints[f'avoid_collision {p}'] = SC.from_constraint(
closest_distance_constraint(actuated_object.pose, obj.pose,
actuated_object_path, p, 0.01),
100)
goal_constraints.update({
f'open_object_{x}': PIDC(s, s, 1)
for x, s in enumerate(gm.free_symbols(target_object.pose))
})
self.look_goal = look_goal if camera_path is not None else None
self.in_contact = gm.less_than(geom_distance, 0.01)
self.controlled_values = controlled_values
self.geom_distance = geom_distance
self.qpb = GQPB(coll_world,
constraints,
goal_constraints,
controlled_values,
visualizer=visualizer)
self.qpb._cb_draw = self.p_internals.f_debug_draw
def __init__(self,
observations,
constraints,
Q=None,
transition_rules=None,
trim_threshold=None):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
constraints (dict): A dict of named constraints that govern the
configuration space of the estimated quantities
Q (matrix, optional): Process noise of the estimated quantities.
Note: Quantities are expected to be ordered alphabetically
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.ordered_vars = [
s for _, s in sorted((str(s), s) for s in state_vars)
]
self.Q = Q if Q is not None else np.zeros(
(len(self.ordered_vars), len(self.ordered_vars)))
st_fn = {}
for s in self.ordered_vars:
st_fn[s] = gm.wrap_expr(s + gm.DiffSymbol(s) * EKFModel.DT_SYM)
if transition_rules is not None:
varset = set(self.ordered_vars).union(
{gm.DiffSymbol(s)
for s in self.ordered_vars}).union({EKFModel.DT_SYM})
for s, r in transition_rules.items():
if s in st_fn:
if len(gm.free_symbols(r).difference(varset)) == 0:
st_fn[s] = gm.wrap_expr(r)
else:
print(
f'Dropping rule "{s}: {r}". Symbols missing from state: {gm.free_symbols(r).difference(varset)}'
)
control_vars = union([gm.free_symbols(r) for r in st_fn.values()]) \
.difference(self.ordered_vars) \
.difference({EKFModel.DT_SYM})
self.ordered_controls = [
s for _, s in sorted((str(s), s) for s in control_vars)
]
# State as column vector n * 1
temp_g_fn = gm.Matrix(
[gm.extract_expr(st_fn[s]) for s in self.ordered_vars])
self.g_fn = gm.speed_up(temp_g_fn, [EKFModel.DT_SYM] +
self.ordered_vars + self.ordered_controls)
temp_g_prime_fn = gm.Matrix([[
gm.extract_expr(st_fn[s][d]) if d in st_fn[s] else 0
for d in self.ordered_controls
] for s in self.ordered_vars])
self.g_prime_fn = gm.speed_up(temp_g_prime_fn,
[EKFModel.DT_SYM] + self.ordered_vars +
self.ordered_controls)
self.obs_labels = []
self.takers = []
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(
sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(
sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []),
o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
self.obs_labels.append('{}_{}_{}'.format(
o_label, *coords))
flat_obs.append(gm.wrap_expr(c))
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers.append((o_label, indices))
else:
self.obs_labels.append(o_label)
flat_obs.append(gm.wrap_expr(o))
self.takers.append((o_label, [0]))
temp_h_fn = gm.Matrix([gm.extract_expr(o) for o in flat_obs])
self.h_fn = gm.speed_up(temp_h_fn, self.ordered_vars)
temp_h_prime_fn = gm.Matrix([[
gm.extract_expr(o[d]) if d in o else 0
for d in self.ordered_controls
] for o in flat_obs])
self.h_prime_fn = gm.speed_up(temp_h_prime_fn, self.ordered_vars)
state_constraints = {}
for n, c in constraints.items():
if gm.is_symbol(c.expr):
s = gm.free_symbols(c.expr).pop()
fs = gm.free_symbols(c.lower).union(gm.free_symbols(c.upper))
if len(fs.difference({s})) == 0:
state_constraints[s] = (float(gm.subs(c.lower, {s: 0})),
float(gm.subs(c.upper, {s: 0})))
self.state_bounds = np.array([
state_constraints[s]
if s in state_constraints else [-np.pi, np.pi]
for s in self.ordered_vars
])
self.R = None # np.zeros((len(self.obs_labels), len(self.obs_labels)))
self.trim_threshold = trim_threshold
'sink_area_dish_washer_door_handle',
'sink_area_left_bottom_drawer_handle',
'sink_area_left_middle_drawer_handle',
'sink_area_left_upper_drawer_handle',
'sink_area_trash_drawer_handle'
]
# QP CONFIGURTION
base_joint = km.get_data(f'{robot}/joints/to_world')
base_link = km.get_data(f'{robot}/links/{urdf_model.get_root()}')
joint_symbols = [
j.position for j in km.get_data(f'{robot}/joints').values()
if hasattr(j, 'position') and gm.is_symbol(j.position)
]
robot_controlled_symbols = {gm.DiffSymbol(j) for j in joint_symbols}
integration_rules = None
if isinstance(base_joint, DiffDriveJoint):
robot_controlled_symbols |= {
base_joint.l_wheel_vel, base_joint.r_wheel_vel
}
# print(pos_of(km.get_data('fetch/links/base_link/pose'))[0][base_joint.l_wheel_vel])
# exit()
integration_rules = {
base_joint.x_pos:
base_joint.x_pos + DT_SYM *
(base_joint.r_wheel_vel * gm.cos(base_joint.a_pos) *
base_joint.wheel_radius * 0.5 + base_joint.l_wheel_vel *
gm.cos(base_joint.a_pos) * base_joint.wheel_radius * 0.5),
visualizer.draw_poses('pre_open_world', gm.eye(4), 0.2, 0.01, [
gm.subs(goal_pose, {s: 0
for s in gm.free_symbols(goal_pose)}),
gm.subs(eef.pose, q_ik_goal)
])
visualizer.draw_world('pre_open_world', world, g=0.6, b=0.6)
visualizer.render('pre_open_world')
# Build Door opening problem
grasp_err_rot = gm.norm(gm.rot_of(goal_pose - eef.pose).elements())
grasp_err_lin = gm.norm(gm.pos_of(goal_pose - eef.pose))
active_symbols = {s for s in gm.free_symbols(eef.pose) if gm.get_symbol_type(s) == gm.TYPE_POSITION}\
.union({door_position, handle_position})
controlled_symbols = {gm.DiffSymbol(s) for s in active_symbols}
controlled_values, constraints = generate_controlled_values(
km.get_constraints_by_symbols(
controlled_symbols.union(active_symbols)),
controlled_symbols)
# Static grasp goal
goal_grasp_lin = SoftConstraint(-grasp_err_lin, -grasp_err_lin,
100.0, grasp_err_lin)
goal_grasp_ang = SoftConstraint(-grasp_err_rot, -grasp_err_rot,
10.0, grasp_err_rot)
goal_door_angle = SoftConstraint(math.pi * 0.45 - door_position,
math.pi * 0.45 - door_position,
1.0, door_position)
def __init__(self, km, observations, transition_rules=None, max_iterations=20, num_samples=7):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
km (ArticulationModel): Articulation model to query for constraints
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.num_samples = num_samples
self.ordered_vars, \
self.transition_fn, \
self.transition_args = generate_transition_function(QPStateModel.DT_SYM,
state_vars,
transition_rules)
self.command_vars = {s for s in self.transition_args
if s not in state_vars and str(s) != str(QPStateModel.DT_SYM)}
obs_constraints = {}
obs_switch_vars = {}
# State as column vector n * 1
self.switch_vars = {}
self._obs_state = {}
self.obs_vars = {}
self.takers = {}
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
obs_switch_var = gm.Symbol(f'{o_label}_observed')
self.switch_vars[o_label] = obs_switch_var
if o_label not in obs_constraints:
obs_constraints[o_label] = {}
if o_label not in self.obs_vars:
self.obs_vars[o_label] = []
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
obs_symbol = gm.Symbol('{}_{}_{}'.format(o_label, *coords))
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - (1 - obs_switch_var) * 1e3,
-obs_error + (1 - obs_switch_var) * 1e3, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers[o_label] = indices
else:
obs_symbol = gm.Symbol(f'{o_label}_value')
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - obs_switch_var * 1e9,
-obs_error + obs_switch_var * 1e9, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
self.takers[o_label] = [0]
state_constraints = km.get_constraints_by_symbols(state_vars)
cvs, hard_constraints = generate_controlled_values(state_constraints,
{gm.DiffSymbol(s) for s in state_vars
if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN})
flat_obs_constraints = dict(sum([list(oc.items()) for oc in obs_constraints.values()], []))
self.qp = TQPB(hard_constraints, flat_obs_constraints, cvs)
st_bound_vars, st_bounds, st_unbounded = static_var_bounds(km, state_vars)
self._state = {s: 0 for s in st_unbounded} # np.random.uniform(-1.0, 1.0) for s in st_unbounded}
for vb, (lb, ub) in zip(st_bound_vars, st_bounds):
self._state[vb] = np.random.uniform(lb, ub)
self._state_buffer = []
self._state.update({s: 0 for s in self.transition_args})
self._obs_state = {s: 0 for s in sum(self.obs_vars.values(), [])}
self._obs_count = 0
self._stamp_last_integration = None
self._max_iterations = 10
self._current_error = 1e9
else:
reference_frame = urdf_model.get_root()
visualizer = ROSBPBVisualizer('~vis', base_frame=reference_frame)
model_name = load_localized_model(km, model_path, reference_frame)
km.clean_structure()
km.dispatch_events()
# Robot stuff
eef_link = rospy.get_param('~eef_link', 'r_gripper_tool_frame')
joint_symbols = [j.position for j in km.get_data(f'pr2/joints').values()
if hasattr(j, 'position') and gm.is_symbol(j.position)]
robot_controlled_symbols = {gm.DiffSymbol(j) for j in joint_symbols if 'torso' not in str(j)}
base_symbols = None
if use_base:
base_joint = km.get_data(base_joint_path)
base_symbols = BaseSymbols(base_joint.x_pos, base_joint.y_pos, base_joint.a_pos,
gm.DiffSymbol(base_joint.x_pos),
gm.DiffSymbol(base_joint.y_pos),
gm.DiffSymbol(base_joint.a_pos))
robot_controlled_symbols |= {gm.DiffSymbol(x) for x in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos]}
eef_path = Path(f'pr2/links/{eef_link}')
cam_path = Path('pr2/links/head_mount_link')
resting_pose = {#'l_elbow_flex_joint' : -2.1213,
#'l_shoulder_lift_joint': 1.2963,
#'l_wrist_flex_joint' : -1.16,