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,
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 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, 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 __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
controlled_symbols |= {
get_diff(x)
for x in
[base_joint.x_pos, base_joint.y_pos, base_joint.a_pos]
}
constraints = km.get_constraints_by_symbols(
geom_distance.free_symbols.union(controlled_symbols))
constraints.update(
generate_contact_model(robot_cp, controlled_symbols, object_cp,
contact_normal, obj_pose.free_symbols))
controlled_values, constraints = generate_controlled_values(
constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.2)
print('Controlled values:\n{}'.format('\n'.join(
[str(x) for x in controlled_values.values()])))
print('Additional joint constraints:\n{}'.format('\n'.join([
str(c) for c in constraints.values()
if c.expr in controlled_symbols
])))
in_contact = less_than(geom_distance, 0.01)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.01),
'look_at_obj': SC(-look_goal, -look_goal, 1, look_goal)
}