def generate_push_controller(self):
if self.robot is None:
return
if self._current_target is None:
return
target_symbol = self.target_symbol_map[self._current_target]
pose_path = self._current_target[len(self.urdf_path):] + ('pose', )
obj_pose = pose_path.get_data(self.obj)
joint_symbols = self.robot_joint_symbols.union({target_symbol})
controlled_symbols = self.robot_controlled_symbols.union(
{DiffSymbol(target_symbol)})
# print('Generating push controller for symbols:\n {}'.format('\n '.join([str(s) for s in symbols])))
hard_constraints, geom_distance, self.geom_world, debug_draw = generate_push_closing(
self.km,
self.state,
controlled_symbols,
self.robot_eef.pose,
obj_pose,
self.robot_eef_path,
self._current_target,
'linear', # 'cross',
BULLET_FIXED_OFFSET)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(self.km,
controlled_values,
exp_factor=1.1)
if isinstance(self.base_joint, DiffDriveJoint):
controlled_values[str(self.base_joint.l_wheel_vel)].weight = 0.001
controlled_values[str(self.base_joint.r_wheel_vel)].weight = 0.001
print('\n'.join(sorted([str(s) for s in geom_distance.free_symbols])))
cam_to_obj = pos_of(obj_pose) - pos_of(self.robot_camera.pose)
lookat_dot = 1 - dot_product(self.robot_camera.pose * vector3(1, 0, 0),
cam_to_obj) / norm(cam_to_obj)
soft_constraints = {
'reach {}'.format(self._current_target):
PIDC(geom_distance, geom_distance, 1, k_i=0.02),
'close {}'.format(self._current_target):
PIDC(target_symbol, target_symbol, 1, k_i=0.06),
'lookat {}'.format(self._current_target):
SC(-lookat_dot, -lookat_dot, 1, lookat_dot)
}
self.soft_constraints = soft_constraints
print('Controlled Values:\n {}'.format('\n '.join(
[str(c) for c in controlled_values.values()])))
self.pushing_controller = GQPB(self.geom_world,
hard_constraints,
soft_constraints,
controlled_values,
visualizer=self.debug_visualizer)
def __init__(self, km,
actuated_pose,
goal_pose,
controlled_symbols,
weight_override=None,
draw_fn=None):
eef = actuated_pose
goal_pos_error = gm.norm(gm.pos_of(eef) - gm.pos_of(goal_pose))
self.eef_rot = eef[:3, :3]
self.goal_rot = goal_pose[:3, :3]
self.rot_error_matrix = self.eef_rot - self.goal_rot
goal_rot_error = gm.norm(self.rot_error_matrix)
rot_align_scale = gm.exp(-3 * goal_rot_error)
goal_constraints = {'align position': SC(-goal_pos_error * rot_align_scale,
-goal_pos_error * rot_align_scale, 10, goal_pos_error),
'align rotation': SC(-goal_rot_error, -goal_rot_error, 1, goal_rot_error)}
self.goal_rot_error = goal_rot_error
constraints = km.get_constraints_by_symbols(gm.free_symbols(eef).union(controlled_symbols))
cvs, constraints = generate_controlled_values(constraints, controlled_symbols, weights=weight_override)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=1.02)
self.draw_fn = draw_fn
self.qp = TQPB(constraints,
goal_constraints,
cvs)
def gen_dv_cvs(km, constraints, controlled_symbols):
cvs, constraints = generate_controlled_values(constraints,
controlled_symbols)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=2.0)
print('\n'.join(f'{cv.symbol}: {cv.weight_id}'
for _, cv in sorted(cvs.items())))
return cvs, constraints
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 generate_idle_controller(self):
if self.robot is None:
return
tucking_constraints = {}
if self._resting_pose is not None:
tucking_constraints = {
'tuck {}'.format(s): SC(p - s, p - s, 1, s)
for s, p in self._resting_pose.items()
if s in self.robot_joint_symbols
}
# 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)
cam_to_poi = self.poi - pos_of(self.robot_camera.pose)
lookat_dot = 1 - dot_product(self.robot_camera.pose * vector3(1, 0, 0),
cam_to_poi) / 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 |= c.expr.free_symbols
joint_symbols = self.robot_joint_symbols.intersection(symbols)
controlled_symbols = self.robot_controlled_symbols
hard_constraints = self.km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
self.geom_world = self.km.get_active_geometry(joint_symbols,
include_static=True)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
self.km, controlled_values)
self.idle_controller = GQPB(self.geom_world,
hard_constraints,
tucking_constraints,
controlled_values,
visualizer=self.debug_visualizer)
def generate_opt_problem(self):
joint_symbols = self.joints
opt_symbols = {DiffSymbol(j) for j in joint_symbols}
hard_constraints = self.km_client.get_constraints_by_symbols(
joint_symbols | opt_symbols)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, opt_symbols)
for mp in self._unintialized_poses:
state = self.integrator.state if self.integrator is not None else {}
te = self.tracked_poses[mp]
state.update({s: 0.0 for s in cm.free_symbols(te.pose)})
state = {} if self.integrator is None else self.integrator.state
self.integrator = CommandIntegrator(TQPB(hard_constraints,
self.soft_constraints,
controlled_values),
start_state=state)
self.integrator.restart('Pose Tracking')
def gen_controlled_values(self, km, constraints, controlled_symbols):
"""Base implementation expected to return a tuple of
controlled values and constraints"""
return generate_controlled_values(constraints, controlled_symbols)
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
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)
goal_handle_angle = SoftConstraint(
math.pi * 0.25 - handle_position,
math.pi * 0.25 - handle_position, 1.0, handle_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
print(spring_constraint)
lock_constraint = Constraint(
old_vel_c.lower, alg_or(greater_than(door_position, 0.1),
button_pushed), old_vel_c.expr)
km.add_constraint(vc_name, lock_constraint)
print(lock_constraint)
goal = 1.57 - door_position
controlled_symbols = {get_diff_symbol(button_position), door_velocity}
constraints = km.get_constraints_by_symbols(
controlled_symbols.union({button_position}))
print('----\n{}'.format('\n'.join([str(x) for x in constraints.values()])))
cvs, constraints = generate_controlled_values(constraints,
controlled_symbols)
print('Retrieved Constraints:\n{}'.format('\n'.join(
['{}:\n {}'.format(k, c) for k, c in constraints.items()])))
integrator = CommandIntegrator(TQPB(
constraints, {'goal': SC(-0.1, -0.1, 1, button_position)}, cvs),
start_state={button_position: 0},
recorded_terms={
'lock_lower': lock_constraint.lower,
'lock_upper': lock_constraint.upper,
'spring_lower': spring_constraint.lower,
})
integrator.restart('Door spring')
integrator.run(dt=0.05)
