'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') integrator.run(int_factor) # DRAW draw_recorders([integrator.recorder] + split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8, 4).savefig('{}/fetch_sandbox_plots.png'.format(plot_dir)) trajectory = { Path(j)[-1][:-2]: [0.0] * len(integrator.recorder.data.values()[0]) for j in joint_symbols } trajectory.update({ Path(se.Symbol(j))[-1][:-2]: d for j, d in integrator.recorder.data.items() }) base_trajectory = list( zip(integrator.sym_recorder.data['location_x'], integrator.sym_recorder.data['location_y'], integrator.sym_recorder.data['location_z'], integrator.sym_recorder.data['rotation_a']))
'l_forearm_roll_joint': 0.0, 'l_wrist_flex_joint': -1.05, 'r_elbow_flex_joint': -2.1213, 'r_shoulder_lift_joint': 1.2963, 'r_shoulder_pan_joint': 0.0, 'r_upper_arm_roll_joint': 0.0, 'r_forearm_roll_joint': 0.0, 'r_wrist_flex_joint': -1.05, 'torso_lift_joint': 0.16825 } closer = ObsessiveObjectCloser( Path('iai_oven_area'), Path('pr2/links/r_gripper_r_finger_tip_link'), Path('pr2/links/head_mount_link'), '/tracked/state', '/pr2', resting_pose=resting_pose) while not rospy.is_shutdown(): rospy.sleep(1000) if closer.pushing_controller is not None and PANDA_LOGGING: recW, recB, recC, recs = convert_qp_builder_log( closer.pushing_controller) draw_recorders([recW, recB, recC] + [ r for _, r in sorted(recs.items()) ], 1, 12, 6).savefig( res_pkg_path( 'package://kineverse_experiment_world/test/keep_contact.png'))
'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( 'package://kineverse_experiment_world/plots/door_opening_terms.png' )) if PANDA_LOGGING: rec_w, rec_b, rec_c, recs = convert_qp_builder_log( integrator.qp_builder) draw_recorders([rec_b, rec_c] + [ r for _, r in sorted(recs.items()) ], 1, 8, 4).savefig( res_pkg_path( 'package://kineverse_experiment_world/plots/door_opening.png' )) except Exception as e: traceback.print_exception(type(e), e, e.__traceback__) print(f'Motion generation crashed:\n{e}')
rec_iter.data = { iter_columns[0]: df_performance[iter_columns[0]].to_list() } rec_iter.data[iter_columns[1]] = (df_performance[iter_columns[1]] * 1000).to_list() rec_iter.compute_limits() rec_iter.set_grid(True) # rec_iter.set_xspace(-0.1, df_performance['DoF'].max() + 0.1) rec_iter.set_ytitle('n iterations | ms') rec_iter.set_xtitle('Degrees of Freedom') rec_iter.set_xlabels([str(int(x)) for x in df_performance['DoF']]) columns = ['Mean Error', 'SD Error', 'Min Error', 'Max Error'] rec_err = ValueRecorder('Error Evaluation', *columns) rec_err.data = {x: df_error[x].to_list() for x in columns} rec_err.compute_limits() rec_err.set_grid(True) rec_err.set_xspace(-0.1, len(df_error) - 0.9) rec_err.set_xlabels([ '{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format( x, y) for x, y in zip(df_error['Linear SD'], df_error['Angular SD']) ]) rec_err.set_marker('.') # rec_err.set_xlabels([]) # ['{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format(x, y) for x, y in df.iloc[:,:2].to_numpy()]) #rec_iter.set_xlabels(['{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format(x, y) for x, y in df.iloc[:,:2].to_numpy()]) draw_recorders([rec_err, rec_iter], 1.0, 8, 3).savefig('{}.png'.format(args[0][:-4]))
matrix = matrix.col_join(f) x_labels.append(len(matrix.free_symbols)) cython_m = llvm.speed_up(matrix, matrix.free_symbols) avg_d = 0.0 avg_l = 0.0 for i in range(iterations): state = {s: random.random() for s in matrix.free_symbols} start = time.time() matrix.subs(state) avg_d += time.time() - start start = time.time() cython_m(**{str(s): v for s, v in state.items()}) avg_l += time.time() - start rec_d.log_data('direct', avg_d / iterations) rec_d.log_data('llvm', avg_l / iterations) # rec_d.set_xspace(x_labels[0], x_labels[-1]) rec_d.set_xspace(-5, 105) rec_d.set_xtitle('Number of Transformation Matrices') rec_d.set_ytitle('Time (s)') draw_recorders([rec_d], ratio=1, plot_width=8, plot_height=4).savefig('llvm_speedtest.png')
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) traj_vis.shutdown()
rec_err = ValueRecorder('Error Evaluation', *columns) rec_err.data = { x: v for x, v in zip(columns, np.vstack(estimate_data_points).T) } rec_err.compute_limits() rec_err.set_grid(True) rec_err.set_xspace(-0.1, len(estimate_data_points) - 0.9) rec_err.set_xlabels(noise_labels) rec_err.set_marker('.') np_p = df_s.to_numpy() * 1000 iter_columns = ['Mean Duration', 'SD Duration'] # , 'Min Duration', 'Max Duration'] rec_iter = ValueRecorder('Iterations', *iter_columns) rec_iter.data = { iter_columns[0]: np_p.mean(axis=0), iter_columns[1]: np_p.std(axis=0), } # iter_columns[2]: np_p.min(axis=0), # iter_columns[3]: np_p.max(axis=0)} rec_iter.compute_limits() rec_iter.set_grid(True) # rec_iter.set_xspace(-0.1, df_performance['DoF'].max() + 0.1) rec_iter.set_ytitle('ms') rec_iter.set_xtitle('Degrees of Freedom') rec_iter.set_xlabels([str(int(x)) for x in range(1, np_p.shape[1] + 1)]) draw_recorders([rec_err, rec_iter], 1.0, 8, 3).savefig(args.output)
) in symbolic_types and u_const_name not in generated_constraints: new_goals[u_const_name] = SoftConstraint( less_than(c.upper, 0), 1000, goal.weight, c.upper) generated_constraints.add(u_const_name) elif diff_sign[s] < 0 and type( c.lower ) in symbolic_types and l_const_name not in generated_constraints: new_goals[l_const_name] = SoftConstraint( -1000, -greater_than(c.lower, 0), goal.weight, c.lower) generated_constraints.add(l_const_name) final_goals.update( lock_explorer(km, state, new_goals, generated_constraints)) return final_goals if __name__ == '__main__': scenario = LockboxOpeningGenerator() scenario.run(0.19) draw_recorders([ scenario.value_recorder, scenario.vel_recorder, scenario.symbol_recorder ], 4.0 / 9.0, 10, 3).savefig( res_pkg_path( 'package://kineverse_experiment_world/test/plots/lockbox_opening.png' ))
# 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: integrator.run(int_factor, 500) except Exception as e: print(e) # DRAW print('Drawing recorders') draw_recorders([ filter_contact_symbols(integrator.recorder, integrator.qp_builder), integrator.sym_recorder ], 4.0 / 9.0, 8, 4).savefig('{}/fetch_sandbox_{}_plots.png'.format( plot_dir, part)) if PANDA_LOGGING: rec_w, rec_b, rec_c, recs = convert_qp_builder_log( integrator.qp_builder) draw_recorders( [rec_b, rec_c] + [r for _, r in sorted(recs.items())], 1, 8, 4).savefig('{}/fetch_sandbox_{}_constraints.png'.format( plot_dir, part)) if False: traj_vis.visualize(integrator.recorder.data, hz=50) pass traj_vis.shutdown()