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 __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_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')
y1: y1 + DT_SYM * (rw_v * sin(a1) * r * 0.5 + lw_v * sin(a1) * r * 0.5), a1: a1 + DT_SYM * (rw_v * (r / L) + lw_v * (- r / L)), # x2: x2 + DT_SYM * l_v * cos(a2), # y2: y2 + DT_SYM * l_v * sin(a2), # a2: a2 + DT_SYM * a_v } goal = point3(1, 1, 0) diff_dist = norm(dot(diff_drive, point3(0.1, 0, 0)) - goal) # # md_dist = norm(dot(my_drive, point3(0.1, 0, 0)) - goal) # qpb = TQPB({}, {'goal diff_drive': SC(-diff_dist, -diff_dist, 1, diff_dist)}, # 'goal my_drive': SC(-md_dist, -md_dist, 1, md_dist)}, {str(rw_v): CV(-r_limit, r_limit, rw_v, 0.001), str(lw_v): CV(-r_limit, r_limit, lw_v, 0.001), # str(l_v): CV(-1, 1, l_v, 0.001), # str(a_v): CV(-a_limit, a_limit, a_v, 0.001) }) full_diff_points = [] md_points = [] cmds = [ {rw_v: -0.8, lw_v: -0.8}, {rw_v: -1.0, lw_v: -0.0}, {rw_v: -0.0, lw_v: -1.0}, {rw_v: 0.8, lw_v: 0.8}, {rw_v: 1.0, lw_v: 0.0},
from kineverse.motion.integrator import CommandIntegrator from kineverse.utils import res_pkg_path if __name__ == '__main__': eef = point3(1,0,0) goal = rotation3_rpy(0, -0.8, 2.56) * point3(1, 0, 0) rpy_model = rotation3_rpy(Position('roll'), Position('pitch'), Position('yaw')) * eef axis = vector3(Position('x'), Position('y'), 0) #Position('z')) ax_model = rotation3_axis_angle(axis / (norm(axis) + 1e-4), norm(axis)) * eef goal_rpy = SC(-norm(goal - rpy_model), -norm(goal - rpy_model), 1, norm(goal - rpy_model)) goal_ax = SC(-norm(goal - ax_model), -norm(goal - ax_model), 1, norm(goal - ax_model)) rpy_integrator = CommandIntegrator(TQPB({}, {'rpy': goal_rpy}, {str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in rpy_model.free_symbols}), recorded_terms={'dist rpy': norm(goal - rpy_model)}) ax_integrator = CommandIntegrator(TQPB({}, {'ax': goal_ax}, {str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in ax_model.free_symbols}), recorded_terms={'dist ax': norm(goal - ax_model)}) rpy_integrator.restart('Convergence Test: RPY vs AxAngle') rpy_integrator.run() ax_integrator.restart('Convergence Test: RPY vs AxAngle') ax_integrator.run() draw_recorders([rpy_integrator.recorder, ax_integrator.recorder, rpy_integrator.sym_recorder, ax_integrator.sym_recorder], 1, 4, 4).savefig('{}/rpy_vs_axis_angle.png'.format(res_pkg_path('package://kineverse_experiment_world/test')))
constraints = {k: c for k, c in constraints.items() if k not in to_remove} for s in controlled_symbols: if str(s) not in controlled_values: controlled_values[str(s)] = ControlledValue(-1e9, 1e9, s, 0.01) print('\n'.join(controlled_values.keys())) goal_constraints = { 'reach_point': SoftConstraint(-dist, -dist, 1, dist), 'look_at_eef': SoftConstraint(-look_goal, -look_goal, 1, look_goal) } base_link = km.get_data('fetch/links/base_link') integrator = CommandIntegrator(TQPB( constraints, goal_constraints, controlled_values ), { roomba_joint.x_pos: roomba_joint.x_pos + DT_SYM * get_diff(pos_of(base_link.to_parent)[0].subs({roomba_joint.x_pos: 0})), roomba_joint.y_pos: roomba_joint.y_pos + DT_SYM * get_diff(pos_of(base_link.to_parent)[1].subs({roomba_joint.y_pos: 0})), roomba_joint.z_pos: roomba_joint.z_pos + DT_SYM * get_diff(pos_of(base_link.to_parent)[2].subs({roomba_joint.z_pos: 0})), roomba_joint.a_pos: roomba_joint.a_pos + DT_SYM * roomba_joint.ang_vel }, recorded_terms={ 'distance': dist.expr,
a = vector3(s_x, s_y, s_z) rs[k] = rotation3_axis_angle(a / (norm(a) + 1e-4), norm(a)) rs_eval[k] = cm.speed_up(rs[k], free_symbols(rs[k])) results = [] for x, r_goal in enumerate(random_rot_uniform(r_points)): goal_ax, goal_a = axis_angle_from_matrix(r_goal) goal_axa = goal_ax * goal_a t_results = {} for k, m in methods.items(): r_ctrl = rs[k] constraints = m(r_ctrl, r_goal) integrator = CommandIntegrator(TQPB({}, constraints, {str(s): CV(-1, 1, DiffSymbol(s), 1e-3) for s in sum([list(free_symbols(c.expr)) for c in constraints.values()], [])}), start_state={s_x: 1, s_y: 0, s_z: 0}, equilibrium=0.001)#, # recorded_terms=recorded_terms) integrator.restart('Convergence Test {} {}'.format(x, k)) # try: t_start = Time.now() integrator.run(step, 100) t_per_it = (Time.now() - t_start).to_sec() / integrator.current_iteration final_r = rs_eval[k](**{str(s): v for s, v in integrator.state.items()}) final_ax, final_a = axis_angle_from_matrix(final_r) final_axa = final_ax * final_a error_9d = sum([np.abs(float(x)) for x in (final_r - r_goal).elements()]) error_axa_sym = norm(final_axa - goal_axa) # print(final_ax, final_a, type(final_a)) error_axa = float(error_axa_sym)
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(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) integrator.recorder.add_threshold('Door release', point_of_release, 'b') plot_dir = res_pkg_path('package://kineverse/test/plots') draw_recorders([integrator.recorder] + split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,