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)