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 run(self, integration_step=0.02, max_iterations=200): integrator = CommandIntegrator(self.qp_type(self.hard_constraints, self.soft_constraints, self.controlled_values), self.int_rules, self.start_state, self.recorded_terms) integrator.restart(self.name) integrator.run(integration_step, max_iterations) self.value_recorder = integrator.recorder self.symbol_recorder = integrator.sym_recorder
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')))
for c in pushing_controller.controlled_values.values() }) # start_state.update({s: 1.84 for s in gm.free_symbols(obj.pose)}) start_state.update({s: 0.4 for s in gm.free_symbols(obj.pose)}) printed_exprs['relative_vel'] = gm.norm( pushing_controller.p_internals.relative_pos) integrator = CommandIntegrator(pushing_controller.qpb, integration_rules, equilibrium=0.0004, start_state=start_state, recorded_terms={ 'distance': pushing_controller.geom_distance, 'gaze_align': pushing_controller.look_goal, 'in contact': pushing_controller.in_contact, 'location_x': base_joint.x_pos, 'location_y': base_joint.y_pos, 'rotation_a': base_joint.a_pos }, printed_exprs={}) # RUN int_factor = 0.02 integrator.restart(f'{robot} Cartesian Goal Example') # print('\n'.join('{}: {}'.format(s, r) for s, r in integrator.integration_rules.items())) try: start = Time.now() integrator.run(int_factor,
'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, 'gaze_align': look_goal.expr, 'location_x': roomba_joint.x_pos, 'location_y': roomba_joint.y_pos, 'location_z': roomba_joint.z_pos, 'rotation_a': roomba_joint.a_pos }) # RUN int_factor = 0.02 integrator.restart('Fetch Cartesian Goal Example')
class TrackerNode(object): def __init__(self, js_topic, obs_topic, integration_factor=0.05, iterations=30, visualize=False, use_timer=True): self.km_client = GeometryModel() # ModelClient(GeometryModel) self._js_msg = ValueMapMsg() self._integration_factor = integration_factor self._iterations = iterations self._use_timer = use_timer self._unintialized_poses = set() self.aliases = {} self.tracked_poses = {} self.integrator = None self.lock = RLock() self.soft_constraints = {} self.joints = set() self.joint_aliases = {} self.visualizer = ROSVisualizer('/tracker_vis', 'world') if visualize else None self.pub_js = rospy.Publisher(js_topic, ValueMapMsg, queue_size=1) self.sub_obs = rospy.Subscriber(obs_topic, PSAMsg, self.cb_process_obs, queue_size=5) def track(self, model_path, alias): with self.lock: if type(model_path) is not str: model_path = str(model_path) start_tick = len(self.tracked_poses) == 0 and self._use_timer if model_path not in self.tracked_poses: syms = [ Position(Path(model_path) + (x, )) for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'] ] def rf(msg, state): state[syms[0]] = msg.position.x state[syms[1]] = msg.position.y state[syms[2]] = msg.position.z state[syms[3]] = msg.orientation.x state[syms[4]] = msg.orientation.y state[syms[5]] = msg.orientation.z state[syms[6]] = msg.orientation.w def process_model_update(data): self._generate_pose_constraints(model_path, data) axis = vector3(*syms[:3]) self.aliases[alias] = model_path self.tracked_poses[model_path] = TrackerEntry( frame3_quaternion(*syms), rf, process_model_update) self._unintialized_poses.add(model_path) if type(self.km_client) == ModelClient: self.km_client.register_on_model_changed( Path(model_path), process_model_update) else: process_model_update(self.km_client.get_data(model_path)) if start_tick: self.timer = rospy.Timer(rospy.Duration(1.0 / 50), self.cb_tick) def stop_tracking(self, model_path): with self.lock: if type(model_path) is not str: model_path = str(model_path) if model_path in self.tracked_poses: te = self.tracked_poses[model_path] self.km_client.deregister_on_model_changed(te.model_cb) del self.tracked_poses[model_path] del self.soft_constraints['{} align rotation 0'.format( model_path)] del self.soft_constraints['{} align rotation 1'.format( model_path)] del self.soft_constraints['{} align rotation 2'.format( model_path)] del self.soft_constraints['{} align position'.format( model_path)] self.generate_opt_problem() if len(self.tracked_poses) == 0: self.timer.shutdown() @profile def cb_tick(self, timer_event): with self.lock: if self.integrator is not None: self.integrator.run(self._integration_factor, self._iterations, logging=False) self._js_msg.header.stamp = Time.now() self._js_msg.symbol, self._js_msg.value = zip( *[(n, self.integrator.state[s]) for s, n in self.joint_aliases.items()]) self.pub_js.publish(self._js_msg) if self.visualizer is not None: poses = [ t.pose.subs(self.integrator.state) for t in self.tracked_poses.values() ] self.visualizer.begin_draw_cycle('obs_points') self.visualizer.draw_points( 'obs_points', cm.eye(4), 0.1, [pos_of(p) for p in poses if len(p.free_symbols) == 0]) # self.visualizer.draw_poses('obs_points', se.eye(4), 0.1, 0.02, [p for p in poses if len(p.free_symbols) == 0]) self.visualizer.render('obs_points') else: print('Integrator does not exist') @profile def cb_process_obs(self, poses_msg): # print('Got new observation') with self.lock: for pose_msg in poses_msg.poses: if pose_msg.header.frame_id in self.aliases and self.integrator is not None: self.tracked_poses[self.aliases[ pose_msg.header.frame_id]].update_state( pose_msg.pose, self.integrator.state) if self.aliases[pose_msg.header. frame_id] in self._unintialized_poses: self._unintialized_poses.remove( self.aliases[pose_msg.header.frame_id]) def _generate_pose_constraints(self, str_path, model): with self.lock: if str_path in self.tracked_poses: align_rotation = '{} align rotation'.format(str_path) align_position = '{} align position'.format(str_path) if model is not None: m_free_symbols = cm.free_symbols(model.pose) if len(m_free_symbols) > 0: te = self.tracked_poses[str_path] self.joints |= m_free_symbols self.joint_aliases = {s: str(s) for s in self.joints} r_dist = norm( cm.rot_of(model.pose) - cm.rot_of(te.pose)) self.soft_constraints[align_rotation] = SC( -r_dist, -r_dist, 1, r_dist) # print(align_position) # print(model.pose) dist = norm(cm.pos_of(model.pose) - cm.pos_of(te.pose)) # print('Distance expression:\n{}'.format(dist)) # print('Distance expression symbol overlap:\n{}'.format(m_free_symbols.intersection(cm.free_symbols(dist)))) # for s in m_free_symbols: # print('Diff w.r.t {}:\n{}'.format(s, cm.diff(dist, s))) self.soft_constraints[align_position] = SC( -dist, -dist, 1, dist) self.generate_opt_problem() # Avoid crashes due to insufficient perception data. This is not fully correct. if str_path in self._unintialized_poses: state = self.integrator.state.copy( ) if self.integrator is not None else {} state.update({ s: 0.0 for s in m_free_symbols if s not in state }) null_pose = cm.subs(model.pose, state) pos = cm.pos_of(null_pose) quat = real_quat_from_matrix(cm.rot_of(null_pose)) msg = PoseMsg() msg.position.x = pos[0] msg.position.y = pos[1] msg.position.z = pos[2] msg.orientation.x = quat[0] msg.orientation.y = quat[1] msg.orientation.z = quat[2] msg.orientation.w = quat[3] te.update_state(msg, self.integrator.state) else: regenerate_problem = False if align_position in self.soft_constraints: del self.soft_constraints[align_position] regenerate_problem = True if align_rotation in self.soft_constraints: del self.soft_constraints[align_rotation] regenerate_problem = True if regenerate_problem: self.generate_opt_problem() 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')
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)
'grasp_constraint_ang': goal_grasp_ang, 'goal_door_angle': goal_door_angle, 'goal_handle_angle': goal_handle_angle, # 'goal_lin_vel_alignment': goal_lin_vel_alignment }, controlled_values, visualizer=visualizer) is_unlocked = gm.alg_and(gm.greater_than(door_position, 0.4), gm.less_than(handle_position, 0.15)) integrator = CommandIntegrator( qp, start_state=q_ik_goal, recorded_terms={ 'is_unlocked': is_unlocked, 'handle_position': handle_position, 'door_position': door_position }, printed_exprs={'is_unlocked': is_unlocked}) try: integrator.restart(title='Door opening generator') integrator.run(dt=0.05, max_iterations=500, logging=True, show_progress=False, real_time=True) draw_recorders([integrator.sym_recorder], 2, 8, 4).savefig( res_pkg_path(
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, 4).savefig('{}/microwave_spring.png'.format(plot_dir)) traj_vis.visualize(integrator.recorder.data)
start_state = {s: 0.0 for s in coll_world.free_symbols} start_state.update({s: 0.4 for s in obj_pose.free_symbols}) start_state.update({ Position(Path('fetch') + (k, )): v for k, v in tucked_arm.items() }) integrator = CommandIntegrator( GQPB(coll_world, constraints, goal_constraints, controlled_values, visualizer=visualizer), #integrator = CommandIntegrator(TQPB(constraints, goal_constraints, controlled_values), integration_rules, start_state=start_state, recorded_terms={ 'distance': geom_distance, 'gaze_align': look_goal, 'in contact': in_contact, 'goal': goal_constraints.values()[0].expr, 'location_x': base_joint.x_pos, 'location_y': base_joint.y_pos, 'rotation_a': base_joint.a_pos }) # RUN int_factor = 0.1 integrator.restart('Fetch Cartesian Goal Example') # print('\n'.join(['{}: {}'.format(k, v) for k, v in integrator.integration_rules.items()])) # exit(0) try: