diff --git a/MODULE.bazel b/MODULE.bazel index b2d801bd9c..49787170d0 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -93,7 +93,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "7e99d566b57deff9cfc02b016ae06930c6634da5" + commit = "194a8c9e3e361e24aa0568e5979f0a25b5364bbd" ) diff --git a/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel b/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel deleted file mode 100644 index 10aa865f41..0000000000 --- a/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel +++ /dev/null @@ -1,42 +0,0 @@ -# -*- python -*- -load("@drake//tools/install:install.bzl", "install") - -package(default_visibility = ["//visibility:public"]) - -load( - "@drake//tools/skylark:pybind.bzl", - "drake_pybind_library", - "get_drake_py_installs", - "get_pybind_package_info", - "pybind_py_library", -) - -py_binary( - name = "run_tray_parameter_study", - srcs = ["run_tray_parameter_study.py"], - data = [ - "@drake_models//:franka_description", - "@lcm//:lcm-python", - ], - deps = [ - "//bindings/pydairlib/common", - "//lcmtypes:lcmtypes_robot_py", - ], -) - -py_binary( - name = "parameter_study_analysis", - srcs = ["parameter_study_analysis.py"], - data = [ - ":parameter_study_config.yaml", - "//examples/franka:parameters", - "@drake_models//:franka_description", - "@lcm//:lcm-python", - ], - deps = [ - "//bindings/pydairlib/analysis:mbp_plotting_utils", - "//bindings/pydairlib/common", - "//bindings/pydairlib/lcm:process_lcm_log", - "//lcmtypes:lcmtypes_robot_py", - ], -) diff --git a/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py b/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py deleted file mode 100644 index 0ea6b5dd01..0000000000 --- a/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py +++ /dev/null @@ -1,41 +0,0 @@ -import numpy as np -import os -import datetime -def gather_theta_batched_branin_data(n_datapoints, - batch_size, - gains_to_randomize, - thetas_to_randomize, - high_level_folder, - params_t=BraninFnParamsTrain): - - assert n_datapoints == int(n_datapoints / batch_size) * batch_size - - num_batches = int(n_datapoints / (batch_size)) - # Each batch is associated with a single set of system intrinsic parameters - intrinsics = np.zeros((num_batches, len(thetas_to_randomize))) - # for each parameter vector, generate $batch_size random gains to evaluate - gains_to_test = np.zeros((num_batches, batch_size, len(gains_to_randomize))) - - subfolder = "branin_data_" + datetime.now().strftime("%b_%d_%Y_%H%M") + "/" - if not os.path.exists(os.path.join(high_level_folder, subfolder)): - os.makedirs(os.path.join(high_level_folder, subfolder), exist_ok=True) - - with h5py.File(os.path.join(high_level_folder, subfolder, "dataset.hdf5"), 'w') as f: - # intrinsic vectors don't actually get used anywhere, but good to save for bookkeeping - f.create_dataset("intrinsics", shape=intrinsics.shape) - f.create_dataset("gains", shape=gains_to_test.shape) - # this system only has 1 performance metric, but in principle this could be any number - f.create_dataset("metrics", shape=(num_batches, batch_size, 1)) - for batch in range(num_batches): - # Generate a random intrinsic vector for this batch - batch_intrinsic_obj = params_t.generate_random(thetas_to_randomize) - f["intrinsics"][batch,...] = batch_intrinsic_obj.get_list() - robot = Branin(batch_intrinsic_obj, gains_to_randomize) - for point in range(batch_size): - # generate a random gain vector - gain = BraninInputs.generate_random(gains_to_randomize).get_list()[gains_to_randomize] - # run the simulation and obtain the performance metrics - metric = robot.evaluate_x(gain) - f["metrics"][batch,point] = metric - f["gains"][batch,point] = gain - print(f"finished batch {batch}") \ No newline at end of file diff --git a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py deleted file mode 100644 index 0e5aab7237..0000000000 --- a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py +++ /dev/null @@ -1,141 +0,0 @@ -import subprocess -import time -from tqdm import * -import lcm -from bindings.pydairlib.common import plot_styler, plotting_utils -import pydairlib.analysis.mbp_plotting_utils as mbp_plots -from bindings.pydairlib.lcm.process_lcm_log import get_log_data -import matplotlib.pyplot as plt -import numpy as np -import yaml -import dairlib - - -def process_c3_channel(data): - t = [] - x = [] - for msg in data: - t.append(msg.utime / 1e6) - x.append(msg.state) - t = np.array(t) - x = np.array(x) - - return {'t': t, - 'x': x} - -def load_c3_channels(data, c3_target_state_channel, c3_actual_state_channel, c3_debug_output_channel): - c3_target = process_c3_channel(data[c3_target_state_channel]) - c3_actual = process_c3_channel(data[c3_actual_state_channel]) - c3_output = mbp_plots.process_c3_debug(data[c3_debug_output_channel]) - return c3_target, c3_actual, c3_output - -def load_lcm_logs(): - config_file = 'parameter_study_config.yaml' - parameters_directory = 'examples/franka/parameters/' - lcm_channels_sim = 'lcm_channels_simulation.yaml' - - with open(parameters_directory + lcm_channels_sim) as f: - filedata = f.read() - lcm_channels = yaml.load(filedata) - - with open('bindings/pydairlib/analysis/tray_balance_study/' + config_file) as f: - filedata = f.read() - param_study = yaml.load(filedata) - - start_time = 0 - duration = -1 - - c3_channels = {lcm_channels['c3_target_state_channel']: dairlib.lcmt_c3_state, - lcm_channels['c3_actual_state_channel']: dairlib.lcmt_c3_state, - lcm_channels['c3_debug_output_channel']: dairlib.lcmt_c3_output} - callback = load_c3_channels - mass_range = np.arange(0.5, 2.0, 0.05) - # mu_range = np.arange(0.3, 0.8, 0.01) - effective_mu_range = np.arange(0.1, 0.71, 0.1) - - print(effective_mu_range.shape[0]) - all_successes = np.zeros((effective_mu_range.shape[0], 3)) - for i in range(effective_mu_range.shape[0]): - print('%02d', effective_mu_range[i]) - # print(mass_range[i]) - # ps = plot_styler.PlotStyler(nrows=2) - - successes = np.zeros(3) - - for j in range(5): - # ps = plot_styler.PlotStyler() - log_filename = param_study['results_folder'] + param_study['parameter'][2] + '/simlog-' + '%02d_%1d' % (i, j) - log = lcm.EventLog(log_filename, "r") - c3_target, c3_actual, c3_output = \ - get_log_data(log, c3_channels, start_time, duration, callback, False, - lcm_channels['c3_target_state_channel'], lcm_channels['c3_actual_state_channel'], - lcm_channels['c3_debug_output_channel']) - length = min(c3_target['t'].shape[0], c3_actual['t'].shape[0]) - # ps.plot(c3_actual['t'], c3_actual['x'][:, 8:9], subplot_index=0) - # ps.plot(c3_actual['t'], c3_actual['x'][:, 9:10], subplot_index=0) - # ps.plot(c3_actual['t'], c3_actual['x'][:, 1:2], subplot_index=0) - # ps.plot(c3_actual['t'], c3_actual['x'][:, 2:3], subplot_index=0) - first_target = np.array([0.45, 0, 0.485]) - second_target = np.array([0.45, 0, 0.6]) - third_target = np.array([0.7, 0.0, 0.485]) - reached_first_target = np.any(np.all(np.isclose(c3_target['x'][:, 7:10], second_target, atol=1e-3), axis=1)) - reached_second_target = np.any(np.all(np.isclose(c3_target['x'][:, 7:10], third_target, atol=1e-3), axis=1)) - reached_third_target = reached_second_target and np.linalg.norm(c3_target['x'][:length, 7:10] - c3_actual['x'][:length, 7:10], axis=1)[-1] < 0.1 - task_success = np.array([reached_first_target, reached_second_target, reached_third_target]) - # print('reached_targets: ', task_success) - successes += task_success - # print('reached_second_target: ', reached_second_target) - # print('reached_third_target: ', reached_third_target) - t_c3_slice = slice(c3_output['t'].size) - # mbp_plots.plot_c3_inputs(c3_output, t_c3_slice, 1, ps) - # plt.show() - print(successes) - all_successes[i] = successes - # np.save('target_successes_23', all_successes ) - np.save('target_successes_03_26', all_successes ) - -def plot_logs(): - all_successes = np.load('target_successes_03_25.npy') - bar_width = 0.025 - bar_positions = np.arange(all_successes.shape[0]) - n = all_successes.shape[0] - # mu_range = np.arange(0.3, 0.74, 0.05) - effective_mu_range = np.arange(0.1, 0.71, 0.1) - - plt.rc('legend', fontsize=36) - plt.rc('axes', labelsize=36, titlesize=36) - plt.rc('xtick', labelsize=36) - plt.rc('ytick', labelsize=36) - # Plotting bars for each task - for i in range(n): - # for j in range(3): - # plt.bar(bar_positions[i], all_successes[i, 2] / 30, width=bar_width) - # plt.bar(effective_mu_range[i], all_successes[i, 2] / all_successes[i, 0], width=bar_width, color='grey') - plt.bar(effective_mu_range[i], all_successes[i, 0] / 5, width=bar_width, color='grey') - plt.ylabel('Target Success Rate') - plt.xlabel('Tray/End Effector Friction Coefficient') - - # plt.savefig('/home/yangwill/Pictures/plot_styler/' + 'target_1_success') - # plt.legend() - # for i in range(3): - # plt.bar(bar_positions + i * (n + 1), all_successes[:, i], label=f'Task {i + 1}') - - plt.show() - -def main(): - load_lcm_logs() - # plot_logs() - - - - - - - - - - - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml deleted file mode 100644 index 32819883bb..0000000000 --- a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml +++ /dev/null @@ -1,21 +0,0 @@ -trajectory_path: /home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/ -gains_path: examples/franka/parameters/ -model_path: examples/franka/urdf/ -results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/03_26_24/parameter_study/ -#results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/01_31_24/parameter_study/ -processed_results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/03_26_24/parameter_study/data/ -sim_cmd: bazel-bin/examples/franka/franka_sim -osc_cmd: bazel-bin/examples/franka/franka_osc_controller -c3_cmd: bazel-bin/examples/franka/franka_c3_controller -fix_inertia_cmd: 'python3 -m pydrake.multibody.fix_inertia --in_place examples/franka/urdf/tray_parameter_sweep.sdf' - -nominal_c3_gain_filename: franka_c3_options_rotated_supports.yaml -#nominal_model_filename: tray.sdf -nominal_model_filename: plate_end_effector.urdf - -modified_c3_gain_filename: franka_c3_options_parameter_sweep.yaml -#modified_model_filename: tray_parameter_sweep.sdf -modified_model_filename: plate_end_effector_parameter_sweep.urdf - -parameter: ['mu_c3', 'mass_real', 'mu_real'] - diff --git a/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py b/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py deleted file mode 100644 index 141ba33efe..0000000000 --- a/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py +++ /dev/null @@ -1,111 +0,0 @@ -import subprocess -import time -from tqdm import * -import lcm -import dairlib.lcmt_radio_out - -import numpy as np -import yaml - - -def main(): - config_file = 'parameter_study_config.yaml' - sim_time = 20.0 - cooldown_time = 10.0 - start_time = 0 - realtime_rate = 1.0 - num_trials = 10 - c3_start_up_time = 1.0 - sim_run_time = sim_time / realtime_rate - controller_startup_time = 0.1 - - with open('bindings/pydairlib/analysis/tray_balance_study/' + config_file) as f: - filedata = f.read() - config = yaml.load(filedata) - - # Add an extra second to the runtime of the simulator process to account for start up and stopping time - - lcm_logger_cmd = '' - publisher = lcm.LCM() - - # parameter = 'mu_c3' - # parameter = 'mass_real' - parameter = config['parameter'][2] - - nominal_mu_value = 'mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1]' - # nominal_real_mu_value = '0.4' - nominal_real_mu_value = '' - nominal_tray_mass = '1' - - # mu_range = np.arange(0.3, 0.8, 0.05) - # mass_range = np.arange(0.5, 2.0, 0.05) - effective_mu_range = np.arange(0.1, 0.71, 0.1) - tray_mu = 0.4 - - initial_radio_msg = dairlib.lcmt_radio_out() - start_c3_radio_msg = dairlib.lcmt_radio_out() - initial_radio_msg.channel[13] = 1 - initial_radio_msg.channel[11] = 0 - initial_radio_msg.channel[14] = 1 - start_c3_radio_msg.channel[13] = 1 - start_c3_radio_msg.channel[11] = 0 - start_c3_radio_msg.channel[14] = 0 - - # for i in trange(mu_range.shape[0]): - for i in trange(effective_mu_range.shape[0]): - plate_mu = 0.5 * effective_mu_range[i] / (1 - effective_mu_range[i] / (2 * tray_mu)) - # modified_mu_value = 'mu: [%.2f, %.2f, %.2f, 0.1, 0.1, 0.1, 0.1]' % (mu_range[i], mu_range[i], mu_range[i]) - # modified_mass_value = ' %.1f ' % (mass_range[i]) - # modified_real_mu_value = '%.2f' % (mu_range[i]) - modified_real_mu_value = '' % (plate_mu) - - # f = open(gains_path + gain_filename, 'r') - f = open(config['model_path'] + config['nominal_model_filename'], 'r') - - filedata = f.read() - f.close() - # newdata = filedata.replace(nominal_mu_value, modified_mu_value) - newdata = filedata.replace(nominal_real_mu_value, modified_real_mu_value) - # newdata = filedata.replace(nominal_tray_mass, modified_mass_value) - - # f = open(config['gains_path'] + config['modified_c3_gain_filename.yaml'], 'w') - f = open(config['model_path'] + config['modified_model_filename'], 'w') - f.write(newdata) - f.close() - if parameter == 'mass_real': - fix_inertia_process = subprocess.Popen(config['fix_inertia_cmd'].split(' ')) - time.sleep(1.0) - - for j in trange(num_trials): - log_path = config['results_folder'] + parameter + '/simlog-' + '%02d_%1d' % (i, j) - lcm_logger_cmd = ['lcm-logger', - '-f', - '%s' % log_path, - ] - - osc_process = subprocess.Popen(config['osc_cmd'], stdout=subprocess.DEVNULL, - stderr=subprocess.STDOUT) - c3_process = subprocess.Popen(config['c3_cmd'], stdout=subprocess.DEVNULL, - stderr=subprocess.STDOUT) - sim_process = subprocess.Popen(config['sim_cmd'], stdout=subprocess.DEVNULL, - stderr=subprocess.STDOUT) - time.sleep(controller_startup_time) - publisher.publish("RADIO", initial_radio_msg.encode()) - logger_process = subprocess.Popen(lcm_logger_cmd, stdout=subprocess.DEVNULL, - stderr=subprocess.STDOUT) - time.sleep(c3_start_up_time) - publisher.publish("RADIO", start_c3_radio_msg.encode()) - time.sleep(sim_run_time) - osc_process.kill() - c3_process.kill() - sim_process.kill() - logger_process.kill() - time.sleep(cooldown_time) - - -if __name__ == "__main__": - main() - # construct_success_plot() - # convert_logs_to_costs() - # plot_success() - # plot_costs() diff --git a/bindings/pydairlib/franka/BUILD.bazel b/bindings/pydairlib/franka/BUILD.bazel deleted file mode 100644 index 503916512b..0000000000 --- a/bindings/pydairlib/franka/BUILD.bazel +++ /dev/null @@ -1,94 +0,0 @@ -# -*- python -*- -load("@drake//tools/install:install.bzl", "install") -load( - "@drake//tools/skylark:pybind.bzl", - "drake_pybind_library", - "get_drake_py_installs", - "get_pybind_package_info", - "pybind_py_library", -) - -package(default_visibility = ["//visibility:public"]) - -pybind_py_library( - name = "controllers_py", - cc_deps = [ - "//examples/franka/diagrams:franka_c3_controller_diagram", - "//examples/franka/diagrams:franka_osc_controller_diagram", - "@drake//:drake_shared_library", - ], - cc_so_name = "controllers", - cc_srcs = ["controllers_py.cc"], - py_deps = [ - "@drake//bindings/pydrake", - ":module_py", - ], - py_imports = ["."], -) - -py_binary( - name = "franka_env", - srcs = ["franka_env.py"], - data = [ - "//examples/franka:urdfs", - "@drake_models//:franka_description", - ], - deps = [ - "//bindings/pydairlib/franka", - "//bindings/pydairlib/lcm:lcm_trajectory_py", - "//bindings/pydairlib/systems:framework_py", - "//bindings/pydairlib/systems:primitives_py", - "//bindings/pydairlib/systems:robot_lcm_systems_py", - "//lcmtypes:lcmtypes_robot_py", - "@drake//bindings/pydrake", - ], -) - -py_binary( - name = "generate_dataset", - srcs = ["generate_dataset.py"], - data = [ - ":parameters/dataset_params.yaml", - ], - deps = [ - ":franka_env", - "@drake//bindings/pydrake", - ], -) - -py_binary( - name = "planar_box_example", - srcs = ["planar_box_example.py"], - data = [ - ], - deps = [ - "//bindings/pydairlib/solvers:c3_py", - "@drake//bindings/pydrake", - ], -) - -# This determines how `PYTHONPATH` is configured, and how to install the -# bindings. -PACKAGE_INFO = get_pybind_package_info("//bindings") - -py_library( - name = "module_py", - srcs = [ - "__init__.py", - ], - imports = PACKAGE_INFO.py_imports, - deps = [ - "//bindings/pydairlib:module_py", - ], -) - -PY_LIBRARIES = [ - ":controllers_py", -] - -# Package roll-up (for Bazel dependencies). -py_library( - name = "franka", - imports = PACKAGE_INFO.py_imports, - deps = PY_LIBRARIES, -) diff --git a/bindings/pydairlib/franka/__init__.py b/bindings/pydairlib/franka/__init__.py deleted file mode 100644 index fa9c628350..0000000000 --- a/bindings/pydairlib/franka/__init__.py +++ /dev/null @@ -1 +0,0 @@ -# Importing everything in this directory to this package diff --git a/bindings/pydairlib/franka/controllers_py.cc b/bindings/pydairlib/franka/controllers_py.cc deleted file mode 100644 index 074a2fd55f..0000000000 --- a/bindings/pydairlib/franka/controllers_py.cc +++ /dev/null @@ -1,113 +0,0 @@ -#include -#include -#include - -#include "examples/franka/diagrams/franka_c3_controller_diagram.h" -#include "examples/franka/diagrams/franka_osc_controller_diagram.h" -#include "solvers/c3_options.h" - -namespace py = pybind11; - -namespace dairlib { -namespace pydairlib { - -using examples::controllers::FrankaC3ControllerDiagram; -using examples::controllers::FrankaOSCControllerDiagram; - -PYBIND11_MODULE(controllers, m) { - m.doc() = "Binding controller factories for plate balancing demo"; - - using py_rvp = py::return_value_policy; - py::module::import("pydrake.math"); - - py::class_>( - m, "FrankaOSCControllerDiagram") - .def(py::init(), - py::arg("controller_settings"), py::arg("lcm_channels"), - py::arg("lcm")) - .def("get_input_port_robot_state", - &FrankaOSCControllerDiagram::get_input_port_robot_state, - py_rvp::reference_internal) - .def("get_input_port_end_effector_position", - &FrankaOSCControllerDiagram::get_input_port_end_effector_position, - py_rvp::reference_internal) - .def("get_input_port_end_effector_orientation", - &FrankaOSCControllerDiagram::get_input_port_end_effector_orientation, - py_rvp::reference_internal) - .def("get_input_port_end_effector_force", - &FrankaOSCControllerDiagram::get_input_port_end_effector_force, - py_rvp::reference_internal) - .def("get_input_port_radio", - &FrankaOSCControllerDiagram::get_input_port_radio, - py_rvp::reference_internal) - .def("get_output_port_robot_input", - &FrankaOSCControllerDiagram::get_output_port_robot_input, - py_rvp::reference_internal); - - py::class_>( - m, "FrankaC3ControllerDiagram") - .def(py::init(), - py::arg("controller_settings"), py::arg("c3_options"), - py::arg("lcm_channels"), py::arg("lcm")) - .def("get_input_port_robot_state", - &FrankaC3ControllerDiagram::get_input_port_robot_state, - py_rvp::reference_internal) - .def("get_input_port_object_state", - &FrankaC3ControllerDiagram::get_input_port_object_state, - py_rvp::reference_internal) - .def("get_input_port_radio", - &FrankaC3ControllerDiagram::get_input_port_radio, - py_rvp::reference_internal) - .def("get_output_port_mpc_plan", - &FrankaC3ControllerDiagram::get_output_port_mpc_plan, - py_rvp::reference_internal); - - py::class_(m, "C3Options") - .def(py::init<>()) - .def_readwrite("admm_iter", &C3Options::admm_iter) - .def_readwrite("rho", &C3Options::rho) - .def_readwrite("rho_scale", &C3Options::rho_scale) - .def_readwrite("num_threads", &C3Options::num_threads) - .def_readwrite("delta_option", &C3Options::delta_option) - .def_readwrite("projection_type", &C3Options::projection_type) - .def_readwrite("contact_model", &C3Options::contact_model) - .def_readwrite("warm_start", &C3Options::warm_start) - .def_readwrite("use_predicted_x0", &C3Options::use_predicted_x0) - .def_readwrite("solve_time_filter_alpha", &C3Options::solve_time_filter_alpha) - .def_readwrite("publish_frequency", &C3Options::publish_frequency) - .def_readwrite("world_x_limits", &C3Options::workspace_limits) - .def_readwrite("u_horizontal_limits", &C3Options::u_horizontal_limits) - .def_readwrite("u_vertical_limits", &C3Options::u_vertical_limits) - .def_readwrite("workspace_margins", &C3Options::workspace_margins) - .def_readwrite("N", &C3Options::N) - .def_readwrite("gamma", &C3Options::gamma) - .def_readwrite("q_vector", &C3Options::q_vector) - .def_readwrite("r_vector", &C3Options::r_vector) - .def_readwrite("g_vector", &C3Options::g_vector) - .def_readwrite("g_x", &C3Options::g_x) - .def_readwrite("g_gamma", &C3Options::g_gamma) - .def_readwrite("g_lambda_n", &C3Options::g_lambda_n) - .def_readwrite("g_lambda_t", &C3Options::g_lambda_t) - .def_readwrite("g_lambda", &C3Options::g_lambda) - .def_readwrite("g_u", &C3Options::g_u) - .def_readwrite("u_vector", &C3Options::u_vector) - .def_readwrite("u_x", &C3Options::u_x) - .def_readwrite("u_gamma", &C3Options::u_gamma) - .def_readwrite("u_lambda_n", &C3Options::u_lambda_n) - .def_readwrite("u_lambda_t", &C3Options::u_lambda_t) - .def_readwrite("u_lambda", &C3Options::u_lambda) - .def_readwrite("u_u", &C3Options::u_u) - .def_readwrite("mu", &C3Options::mu) - .def_readwrite("dt", &C3Options::dt) - .def_readwrite("solve_dt", &C3Options::solve_dt) - .def_readwrite("num_friction_directions", &C3Options::num_friction_directions) - .def_readwrite("num_contacts", &C3Options::num_contacts) - .def_readwrite("Q", &C3Options::Q) - .def_readwrite("R", &C3Options::R) - .def_readwrite("G", &C3Options::G) - .def_readwrite("U", &C3Options::U); -} -} // namespace pydairlib -} // namespace dairlib \ No newline at end of file diff --git a/bindings/pydairlib/franka/franka_env.py b/bindings/pydairlib/franka/franka_env.py deleted file mode 100644 index 4ba758f5e1..0000000000 --- a/bindings/pydairlib/franka/franka_env.py +++ /dev/null @@ -1,144 +0,0 @@ -from pydrake.all import * -from pydairlib.franka.controllers import FrankaOSCControllerDiagram, FrankaC3ControllerDiagram, C3Options - -from pydrake.common.yaml import yaml_load -from pydairlib.systems.primitives import * -from pydairlib.systems.robot_lcm_systems import * -from pydairlib.lcm.lcm_trajectory import * -import pydrake.systems.lcm as mut -from franka_env_helper_functions import * -import dairlib - - -def run_sim(intrinsics, gains): - osc_params_file = "examples/franka/parameters/franka_osc_controller_params.yaml" - c3_params_file = "examples/franka/parameters/franka_c3_controller_params.yaml" - lcm_params_file = "examples/franka/parameters/lcm_channels_simulation.yaml" - sim_params_file = "examples/franka/parameters/franka_sim_params.yaml" - - sim_params = yaml_load(filename=sim_params_file) - lcm_params = yaml_load(filename=lcm_params_file) - c3_options = load_c3_options(c3_params_file) - - lcm = DrakeLcm("udpm://239.255.76.67:7667?ttl=0") - builder = DiagramBuilder() - plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_params['dt']) - - parser = Parser(plant) - parser.SetAutoRenaming(True) - - franka_index, = parser.AddModels(FindResourceOrThrow(sim_params['franka_model'])) - end_effector_index, = parser.AddModels(sim_params['end_effector_model']) - tray_index, = parser.AddModels(sim_params['tray_model']) - - T_X_W = RigidTransform(RotationMatrix(), np.zeros(3)) - T_EE_W = RigidTransform(RotationMatrix(), sim_params['tool_attachment_frame']) - - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName('panda_link0'), T_X_W) - frame = plant.GetFrameByName(name='plate', model_instance=end_effector_index) - plant.WeldFrames(plant.GetFrameByName('panda_link7'), - frame, - T_EE_W) - left_support_index, = parser.AddModels(sim_params['left_support_model']) - right_support_index, = parser.AddModels(sim_params['right_support_model']) - T_S1_W = RigidTransform(RollPitchYaw(sim_params['left_support_orientation']), - sim_params['left_support_position']) - T_S2_W = RigidTransform(RollPitchYaw(sim_params['right_support_orientation']), - sim_params['right_support_position']) - plant.WeldFrames(plant.world_frame(), - plant.GetFrameByName("support", left_support_index), - T_S1_W) - plant.WeldFrames(plant.world_frame(), - plant.GetFrameByName("support", right_support_index), - T_S2_W) - plant.Finalize() - - c3_options.publish_frequency = (gains[0] * 5) + 25 - c3_options.Q = (1 + 0.5 * gains[1]) * c3_options.Q - c3_options.R = (1 + 0.5 * gains[2]) * c3_options.R - c3_options.G = (1 + 0.5 * gains[3]) * c3_options.G - c3_options.U = (1 + 0.5 * gains[4]) * c3_options.U - - osc_controller = builder.AddSystem(FrankaOSCControllerDiagram( - "examples/franka/parameters/franka_osc_controller_params.yaml", - "examples/franka/parameters/lcm_channels_simulation.yaml", lcm)) - - c3_controller = builder.AddSystem(FrankaC3ControllerDiagram( - "examples/franka/parameters/franka_c3_controller_params.yaml", c3_options, - "examples/franka/parameters/lcm_channels_simulation.yaml", lcm)) - - passthrough = builder.AddSystem(SubvectorPassThrough(8, 0, 7)) - tray_state_sender = builder.AddSystem(ObjectStateSender(plant, tray_index)) - franka_state_sender = builder.AddSystem(RobotOutputSender(plant, franka_index, False, False)) - - # radio_sub = builder.AddSystem(LcmSubscriberSystem.Make(channel=lcm_params['radio_channel'], lcm_type=dairlib.lcmt_radio_out, lcm=lcm, use_cpp_serializer=True)) - sim_state_logger = builder.AddSystem( - VectorLogSink(plant.num_positions() + plant.num_velocities(), 2 * sim_params['dt'])) - - builder.Connect(c3_controller.get_output_port_mpc_plan(), - osc_controller.get_input_port_end_effector_position()) - builder.Connect(c3_controller.get_output_port_mpc_plan(), - osc_controller.get_input_port_end_effector_orientation()) - builder.Connect(c3_controller.get_output_port_mpc_plan(), - osc_controller.get_input_port_end_effector_force()) - - builder.Connect(osc_controller.get_output_port_robot_input(), - passthrough.get_input_port()) - - builder.Connect(franka_state_sender.get_output_port(), - osc_controller.get_input_port_robot_state()) - builder.Connect(franka_state_sender.get_output_port(), - c3_controller.get_input_port_robot_state()) - builder.Connect(tray_state_sender.get_output_port(), - c3_controller.get_input_ports_object_state()[0]) - - builder.Connect(plant.get_state_output_port(franka_index), - franka_state_sender.get_input_port_state()) - builder.Connect(plant.get_state_output_port(tray_index), - tray_state_sender.get_input_port_state()) - builder.Connect(plant.get_state_output_port(), - sim_state_logger.get_input_port()) - builder.Connect(passthrough.get_output_port(), - plant.get_actuation_input_port()) - - if sim_params['visualize_drake_sim']: - AddDefaultVisualization(builder) - - diagram = builder.Build() - simulator = Simulator(diagram) - - simulator.set_publish_every_time_step(True) - simulator.set_publish_at_initialization(True) - simulator.set_target_realtime_rate(sim_params['realtime_rate']) - - plant_context = diagram.GetMutableSubsystemContext(plant, simulator.get_mutable_context()) - osc_controller_context = diagram.GetMutableSubsystemContext(osc_controller, simulator.get_mutable_context()) - c3_controller_context = diagram.GetMutableSubsystemContext(c3_controller, simulator.get_mutable_context()) - osc_controller.get_input_port_radio().FixValue(osc_controller_context, np.zeros(18)) - c3_controller.get_input_port_radio().FixValue(c3_controller_context, np.zeros(18)) - logger_context = diagram.GetSubsystemContext(sim_state_logger, simulator.get_context()) - - q = np.hstack((sim_params['q_init_franka'], sim_params['q_init_tray'][sim_params['scene_index']])) - v = np.zeros(plant.num_velocities()) - plant.SetPositions(plant_context, q) - - simulator.Initialize() - simulator.AdvanceTo(5.0) - - sim_data = sim_state_logger.GetLog(logger_context) - sim_states = sim_data.data() - sim_times = sim_data.sample_times() - - # reward = compute_reward_sparse(sim_times, sim_states) - reward = compute_reward_mpc(sim_times, sim_states) - # print(reward) - # print(sim_states.data().shape) - # print(sim_states.sample_times().shape) - return reward, sim_states - - -if __name__ == '__main__': - parameters = 2 * np.random.random(5) - 1 # center around 0. [-1, 1] - # reward, sim_states = run_sim(np.zeros(5), np.ones(5)) - reward, sim_states = run_sim(np.zeros(5), parameters) - print(reward) diff --git a/bindings/pydairlib/franka/franka_env_helper_functions.py b/bindings/pydairlib/franka/franka_env_helper_functions.py deleted file mode 100644 index ce01e8fff3..0000000000 --- a/bindings/pydairlib/franka/franka_env_helper_functions.py +++ /dev/null @@ -1,85 +0,0 @@ -from pydairlib.franka.controllers import C3Options -import numpy as np -from pydrake.common.yaml import yaml_load -from pydrake.all import * - -def compute_reward_sparse(times, sim_states): - first_target = np.array([0.45, 0, 0.485]) - second_target = np.array([0.45, 0, 0.6]) - third_target = np.array([0.7, 0.0, 0.485]) - length = min(times.shape[0], times.shape[0]) - reached_first_target = np.any(np.all(np.isclose(sim_states[:, 7:10], second_target, atol=1e-3), axis=1)) - reached_second_target = np.any(np.all(np.isclose(sim_states[:, 7:10], third_target, atol=1e-3), axis=1)) - reached_third_target = reached_second_target and np.linalg.norm(sim_states[:length, 7:10] - sim_states[:length, 7:10], axis=1)[-1] < 0.1 - # return reached_first_target + reached_second_target + reached_third_target - return reached_first_target + reached_second_target + reached_third_target - -def compute_reward_mpc(times, sim_states): - first_target = np.array([0.45, 0, 0.485]) - second_target = np.array([0.45, 0, 0.6]) - third_target = np.array([0.7, 0.0, 0.485]) - neutral_orientation = Quaternion() - cumulative_cost = 0 - for i in range(times.shape[0]): - tray_pos = sim_states[11:14, i] - tray_orientation = sim_states[7:11, i] - tray_quat = Quaternion(tray_orientation / np.linalg.norm(tray_orientation)) - cumulative_cost += np.linalg.norm(tray_pos - first_target) - # tray_quat /= np.linalg.norm(tray_quat.wxyz()) - angle_difference = Quaternion.multiply(neutral_orientation.conjugate(), tray_quat) - cumulative_cost += RotationMatrix(angle_difference).ToAngleAxis().angle() - cumulative_cost /= times.shape[0] - return cumulative_cost - -def load_c3_options(c3_params_file): - c3_params = yaml_load(filename=c3_params_file) - c3_options_file = c3_params['c3_options_file'][c3_params['scene_index']] - c3_options_dict = yaml_load(filename=c3_options_file) - c3_options = C3Options() - c3_options.admm_iter = c3_options_dict['admm_iter'] - c3_options.rho = c3_options_dict['rho'] - c3_options.rho_scale = c3_options_dict['rho_scale'] - c3_options.num_threads = c3_options_dict['num_threads'] - c3_options.delta_option = c3_options_dict['delta_option'] - c3_options.projection_type = c3_options_dict['projection_type'] - c3_options.contact_model = c3_options_dict['contact_model'] - c3_options.warm_start = c3_options_dict['warm_start'] - c3_options.use_predicted_x0 = c3_options_dict['use_predicted_x0'] - c3_options.solve_time_filter_alpha = c3_options_dict['solve_time_filter_alpha'] - c3_options.publish_frequency = c3_options_dict['publish_frequency'] - c3_options.world_x_limits = c3_options_dict['world_x_limits'] - c3_options.world_y_limits = c3_options_dict['world_y_limits'] - c3_options.world_z_limits = c3_options_dict['world_z_limits'] - c3_options.u_horizontal_limits = c3_options_dict['u_horizontal_limits'] - c3_options.u_vertical_limits = c3_options_dict['u_vertical_limits'] - c3_options.workspace_margins = c3_options_dict['workspace_margins'] - c3_options.N = c3_options_dict['N'] - c3_options.gamma = c3_options_dict['gamma'] - c3_options.gamma = c3_options_dict['gamma'] - c3_options.q_vector = c3_options_dict['q_vector'] - c3_options.r_vector = c3_options_dict['r_vector'] - c3_options.g_x = c3_options_dict['g_x'] - c3_options.g_gamma = c3_options_dict['g_gamma'] - c3_options.g_lambda_n = c3_options_dict['g_lambda_n'] - c3_options.g_lambda_t = c3_options_dict['g_lambda_t'] - c3_options.g_lambda = c3_options_dict['g_lambda'] - c3_options.g_u = c3_options_dict['g_u'] - c3_options.u_x = c3_options_dict['u_x'] - c3_options.u_gamma = c3_options_dict['u_gamma'] - c3_options.u_lambda_n = c3_options_dict['u_lambda_n'] - c3_options.u_lambda_t = c3_options_dict['u_lambda_t'] - c3_options.u_lambda = c3_options_dict['u_lambda'] - c3_options.u_u = c3_options_dict['u_u'] - c3_options.gamma = c3_options_dict['gamma'] - c3_options.mu = c3_options_dict['mu'] - c3_options.dt = c3_options_dict['dt'] - c3_options.solve_dt = c3_options_dict['solve_dt'] - c3_options.num_friction_directions = c3_options_dict['num_friction_directions'] - c3_options.num_contacts = c3_options_dict['num_contacts'] - c3_options.Q = c3_options_dict['w_Q'] * np.diag(np.array(c3_options_dict['q_vector'])) - c3_options.R = c3_options_dict['w_R'] * np.diag(np.array(c3_options_dict['r_vector'])) - g_vec = np.hstack((c3_options_dict['g_x'], c3_options_dict['g_lambda'], c3_options_dict['g_u'])) - u_vec = np.hstack((c3_options_dict['u_x'], c3_options_dict['u_lambda'], c3_options_dict['u_u'])) - c3_options.G = c3_options_dict['w_G'] * np.diag(g_vec) - c3_options.U = c3_options_dict['w_U'] * np.diag(u_vec) - return c3_options diff --git a/bindings/pydairlib/franka/generate_dataset.py b/bindings/pydairlib/franka/generate_dataset.py deleted file mode 100644 index f892686be0..0000000000 --- a/bindings/pydairlib/franka/generate_dataset.py +++ /dev/null @@ -1,53 +0,0 @@ -from franka_env import * -from yaml import * -from datetime import datetime -import h5py - -# gains are controller params -# thetas/intrinsics are environment params -def generate_dataset(n_datapoints, - batch_size, - gains_to_randomize, - thetas_to_randomize, - high_level_folder): - assert n_datapoints == int(n_datapoints / batch_size) * batch_size - - num_batches = int(n_datapoints / (batch_size)) - # Each batch is associated with a single set of system intrinsic parameters - intrinsics = np.zeros((num_batches, len(thetas_to_randomize))) - # for each parameter vector, generate $batch_size random gains to evaluate - gains_to_test = np.zeros((num_batches, batch_size, len(gains_to_randomize))) - - subfolder = "occam_data" + datetime.now().strftime("%b_%d_%Y_%H%M") + "/" - if not os.path.exists(os.path.join(high_level_folder, subfolder)): - os.makedirs(os.path.join(high_level_folder, subfolder), exist_ok=True) - - with h5py.File(os.path.join(high_level_folder, subfolder, "dataset.hdf5"), 'w') as f: - # intrinsic vectors don't actually get used anywhere, but good to save for bookkeeping - f.create_dataset("intrinsics", shape=intrinsics.shape) - f.create_dataset("gains", shape=gains_to_test.shape) - # this system only has 1 performance metric, but in principle this could be any number - f.create_dataset("metrics", shape=(num_batches, batch_size, 1)) - # f.create_dataset("states", shape=(num_batches, batch_size, 1)) - for batch in range(num_batches): - # Generate a random intrinsic vector for this batch - # batch_intrinsic_obj = params_t.generate_random(thetas_to_randomize) - f["intrinsics"][batch, ...] = thetas_to_randomize - # robot = Branin(batch_intrinsic_obj, gains_to_randomize) - for point in range(batch_size): - # generate a random gain vector - # gain = BraninInputs.generate_random(gains_to_randomize).get_list()[gains_to_randomize] - gain = 2 * np.random.random(gains_to_randomize.shape[0]) - 1 # center around 0. [-1, 1] - # run the simulation and obtain the performance metrics - # metric = robot.evaluate_x(gain) - metric, states = run_sim(thetas_to_randomize, gain) - f["metrics"][batch, point] = metric - f["gains"][batch, point] = gain - # f["states"][batch, point] = states - print(f"finished batch {batch}") - - -if __name__ == '__main__': - hersh_dataset_params_file = 'bindings/pydairlib/franka/parameters/dataset_params.yaml' - params = yaml_load(filename=hersh_dataset_params_file) - generate_dataset(50 * 64, 64, np.ones(5), np.zeros(5), params['high_level_folder']) diff --git a/bindings/pydairlib/franka/parameters/dataset_params.yaml b/bindings/pydairlib/franka/parameters/dataset_params.yaml deleted file mode 100644 index 77293d3cb3..0000000000 --- a/bindings/pydairlib/franka/parameters/dataset_params.yaml +++ /dev/null @@ -1 +0,0 @@ -high_level_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/occam/ diff --git a/bindings/pydairlib/franka/planar_box_example.py b/bindings/pydairlib/franka/planar_box_example.py deleted file mode 100644 index e3036ee1ef..0000000000 --- a/bindings/pydairlib/franka/planar_box_example.py +++ /dev/null @@ -1,229 +0,0 @@ -import numpy as np -from pydrake.all import ( - AddMultibodyPlantSceneGraph, - ContactVisualizer, - DiagramBuilder, - ExternallyAppliedSpatialForce, - LeafSystem, - List, - MeshcatVisualizer, - ModelVisualizer, - Parser, - Simulator, - SpatialForce, - StartMeshcat, - RigidTransform, - Value, - InitializeAutoDiff, - ModelInstanceIndex, - PiecewisePolynomial, -) - -from c3 import * - - -class Controller(LeafSystem): - def __init__(self, plant, plant_context): - LeafSystem.__init__(self) - forces_cls = Value[List[ExternallyAppliedSpatialForce]] - self.DeclareVectorOutputPort( - "controller_output", 1, self.CalcOutput - ) - self.DeclareVectorInputPort("system_state", plant.num_positions() + plant.num_velocities()) - self.plant = plant - self.plant_context = plant_context - self.c3_options = C3Options() - - lcs_builer = DiagramBuilder() - self.plant_for_lcs, self.scene_graph_for_lcs = AddMultibodyPlantSceneGraph( - lcs_builer, 0.0) - lcs_parser = Parser(self.plant_for_lcs) - self.passive_block_index = lcs_parser.AddModels( - "bindings/pydairlib/franka/urdf/passive_block_lcs.sdf")[0] - self.active_block_index = \ - lcs_parser.AddModels("bindings/pydairlib/franka/urdf/active_block.sdf")[ - 0] - self.plant_for_lcs.WeldFrames(self.plant_for_lcs.world_frame(), - self.plant_for_lcs.GetFrameByName("base", - self.passive_block_index)) - self.plant_for_lcs.Finalize() - - lcs_diagram = lcs_builer.Build() - - diagram_context = lcs_diagram.CreateDefaultContext() - self.context_for_lcs = lcs_diagram.GetMutableSubsystemContext( - self.plant_for_lcs, diagram_context) - self.plant_ad = self.plant_for_lcs.ToAutoDiffXd() - self.context_ad = self.plant_ad.CreateDefaultContext() - - passive_block_contact_points = self.plant_for_lcs.GetCollisionGeometriesForBody( - self.plant_for_lcs.GetBodyByName("passive_block", - self.passive_block_index)) - active_block_contact_points = self.plant_for_lcs.GetCollisionGeometriesForBody( - self.plant_for_lcs.GetBodyByName("active_block", - self.active_block_index)) - self.contact_geoms = list() - for geom_id in passive_block_contact_points: - self.contact_geoms.append((active_block_contact_points[0], geom_id)) - self.num_friction_directions = 1 - self.mu = [0.4, 0.4] - self.dt = 0.05 - self.N = 10 - self.contact_model = ContactModel.kAnitescu - # self.contact_model = ContactModel.kStewartAndTrinkle - self.Q = 50 * np.diag(np.array([1000, 5000, 10, 10, 5, 10, 5, 5])) - self.R = 5 * np.eye(1) - self.G = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 100 * np.ones(4), 5 * np.ones(1)))) - self.U = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 100 * np.ones(4), 5 * np.ones(1)))) - # self.G = 0.1 * np.diag(np.ones(13)) - # self.U = 0.1 * np.diag(np.ones(13)) - - # self.U = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 0.01 * np.ones(8), 5 * np.ones(1)))) - # self.G = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 0.01 * np.ones(8), 5 * np.ones(1)))) - self.c3_options.admm_iter = 2 - self.c3_options.rho = 0 - self.c3_options.rho_scale = 2 - self.c3_options.num_threads = 4 - self.c3_options.delta_option = 1 - self.c3_options.contact_model = "anitescu" - # self.c3_options.contact_model = "stewart_and_trinkle" - self.c3_options.warm_start = 1 - self.c3_options.use_predicted_x0 = 0 - self.c3_options.end_on_qp_step = 0 - self.c3_options.use_robust_formulation = 0 - self.c3_options.solve_time_filter_alpha = 0.95 - self.c3_options.publish_frequency = 0 - self.c3_options.u_horizontal_limits = np.array([-5, 5]) - self.c3_options.u_vertical_limits = np.array([0, 0]) - self.c3_options.workspace_limits = [np.array([1, 0, 0, -0.5, 0.5])] # unused - self.c3_options.workspace_margins = 0.05 - self.c3_options.N = self.N - self.c3_options.gamma = 1.0 - self.c3_options.mu = self.mu - self.c3_options.dt = self.dt - self.c3_options.solve_dt = 0.0 - self.c3_options.num_friction_directions = self.num_friction_directions - self.c3_options.num_contacts = len(self.mu) - self.c3_options.Q = self.Q - self.c3_options.R = self.R - self.c3_options.G = self.G - self.c3_options.U = self.U - self.u = PiecewisePolynomial(np.array([0])) - self.last_update_time = -1 - self.c3_solver = None - self.x_des = np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) - self.predicted_x1 = self.x_des - self.lcs_pred = self.x_des - Qs = [] - for i in range(self.N + 1): - Qs.append(1.0 ** i * self.Q) - # costs = CostMatrices((self.N + 1) * [self.Q], self.N * [self.R], self.N * [self.G], self.N * [self.U]) - self.costs = CostMatrices(Qs, self.N * [self.R], self.N * [self.G], self.N * [self.U]) - - def CalcOutput(self, context, output): - - - if context.get_time() > self.last_update_time + (0.1 * self.dt): - x = self.EvalVectorInput(context, 0) - x0 = x.value() - # u0 = np.zeros(1) - u0 = self.u.value(context.get_time())[0] - x_u = np.hstack((x.value(), u0)) - x_u_ad = InitializeAutoDiff(x_u) - - if context.get_time() // 10 % 2 == 0: - self.x_des = np.array([0.0, -0.3, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) - else: - self.x_des = np.array([0.0, 0.3, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) - print("time: ", context.get_time()) - print("current cost: ", (x.value() - self.x_des).T @ (x.value() - self.x_des)) - print("desired x: ", self.x_des) - print("planned x1: ", self.predicted_x1) - print("lcs pred x1: ", self.lcs_pred) - print("actual x1: ", x.value()) - # import pdb; pdb.set_trace() - self.plant_for_lcs.SetPositionsAndVelocities(self.context_for_lcs, x0) - self.plant_for_lcs.get_actuation_input_port().FixValue(self.context_for_lcs, u0) - self.plant_ad.get_actuation_input_port().FixValue(self.context_ad, x_u_ad[-1]) - lcs = LinearizePlantToLCS(self.plant_for_lcs, self.context_for_lcs, - self.plant_ad, self.context_ad, self.contact_geoms, - self.num_friction_directions, self.mu, self.dt, self.N, - self.contact_model) - - if self.c3_solver == None: - self.c3_solver = C3MIQP(lcs, self.costs, (self.N + 1) * [self.x_des], self.c3_options) - else: - self.c3_solver.UpdateLCS(lcs) - self.c3_solver.UpdateTarget((self.N + 1) * [self.x_des]) - self.c3_solver.Solve(x0) - u_sol = self.c3_solver.GetInputSolution() - x_sol = self.c3_solver.GetStateSolution() - w_sol = self.c3_solver.GetDualWSolution() - delta_sol = self.c3_solver.GetDualDeltaSolution() - z_sol = self.c3_solver.GetFullSolution() - - u_sol = np.array(u_sol) - x_sol = np.array(x_sol) - w_sol = np.array(w_sol) - delta_sol = np.array(delta_sol) - z_sol = np.array(z_sol) - - self.predicted_x1 = x_sol[1, :] - self.lcs_pred = lcs.Simulate(x0, u_sol[0]) - # print(next_pred) - # next_pred = lcs.Simulate(next_pred, u_sol[0]) - # print(next_pred) - # import pdb; pdb.set_trace() - timestamps = context.get_time() + self.dt * np.arange(self.N) - self.u = PiecewisePolynomial.ZeroOrderHold(timestamps, u_sol.T) - # self.u = PiecewisePolynomial.FirstOrderHold(timestamps, np.array(u_sol).T) - self.last_update_time = context.get_time() - output.set_value(self.u.value(context.get_time())) - - -def main(): - # np.set_printoptions(3, threshold=3, suppress=True) - builder = DiagramBuilder() - - plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001) - parser = Parser(plant) - passive_block_index = \ - parser.AddModels("bindings/pydairlib/franka/urdf/passive_block.sdf")[0] - active_block_index = \ - parser.AddModels("bindings/pydairlib/franka/urdf/active_block.sdf")[0] - offset = RigidTransform(np.array([0.0, 0.0, 0.0])) - plant.WeldFrames(plant.world_frame(), - plant.GetFrameByName("base", passive_block_index), offset) - # plant.WeldFrames(plant.world_frame(), - # plant.GetFrameByName("base", active_block_index)) - plant.Finalize() - plant_context = plant.CreateDefaultContext() - meshcat = StartMeshcat() - visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat) - ContactVisualizer.AddToBuilder(builder, plant, meshcat) - controller = builder.AddSystem(Controller(plant, plant_context)) - builder.Connect(controller.get_output_port(), - plant.get_actuation_input_port()) - builder.Connect(plant.get_state_output_port(), - controller.get_input_port()) - diagram = builder.Build() - - meshcat.SetCameraPose(np.array([0, -3.0, 0.5]), np.array([0, 0.0, 0.0])) - simulator = Simulator(diagram) - context = simulator.get_context() - plant.SetPositions(diagram.GetMutableSubsystemContext(plant, - simulator.get_mutable_context()), - np.array([0.0, 0.0, 0.1, 0])) # active x, passive: x, z, theta - simulator.Initialize() - simulator.set_target_realtime_rate(1.0) - meshcat.StartRecording() - simulator.AdvanceTo(40.0) - meshcat.StopRecording() - meshcat.PublishRecording() - with open("planar_box_visualization.html", "r+") as f: - f.write(meshcat.StaticHtml()) - print("done simulating") - - -if __name__ == '__main__': - main() diff --git a/bindings/pydairlib/franka/urdf/active_block.sdf b/bindings/pydairlib/franka/urdf/active_block.sdf deleted file mode 100644 index 0cf5c974ec..0000000000 --- a/bindings/pydairlib/franka/urdf/active_block.sdf +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - 0 0 0 0 0 0 - 1 - - 0.014167 - 0 - 0 - 0.33417 - 0 - 0.34667 - - - - 0 0 0 0 0 0 - - 0.5 0.2 0.2 0.6 - - - - 2.0 0.4 0.1 - - - - - 0 0 0 0 0 0 - - - 2.0 0.4 0.1 - - - - - 3.0e7 - 0.15 - 10 - 0.4 - - - - - world - active_block - - - 1 0 0 - - - -2.0 - 2.0 - 50.0 - - - 0.0 - - - - - - \ No newline at end of file diff --git a/bindings/pydairlib/franka/urdf/passive_block.sdf b/bindings/pydairlib/franka/urdf/passive_block.sdf deleted file mode 100644 index a9b43873f8..0000000000 --- a/bindings/pydairlib/franka/urdf/passive_block.sdf +++ /dev/null @@ -1,125 +0,0 @@ - - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 1 - - 0.014167 - 0 - 0 - 0.014167 - 0 - 0.026667 - - - - 0 0 0 0 0 0 - - 0.1 0.1 0.1 0.6 - - - - 0.4 0.4 0.1 - - - - - 0 0 0 0 0 0 - - - 0.4 0.4 0.1 - - - - - 3.0e7 - 0.15 - 10 - 0.4 - - - - - base - base_x - - - 1 0 0 - - - 0.0 - - - - - base_x - base_xz - - - 0 0 1 - - - 0.0 - - - - - base_xz - passive_block - - - 0 1 0 - - - 0.0 - - - - - - \ No newline at end of file diff --git a/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf b/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf deleted file mode 100644 index e218edb31e..0000000000 --- a/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf +++ /dev/null @@ -1,126 +0,0 @@ - - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 0 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - - - 0 0 0 0 0 0 - 1 - - 0.014167 - 0 - 0 - 0.014167 - 0 - 0.026667 - - - - 0 0 0 0 0 0 - - 0.1 0.1 0.1 0.6 - - - - 0.4 0.4 0.1 - - - - - -0.2 0.0 -0.048 0 0 0 - - - 0.001" - - - - - 0.2 0.0 -0.048 0 0 0 - - - 0.001" - - - - - - base - base_x - - - 1 0 0 - - - 0.0 - - - - - base_x - base_xz - - - 0 0 1 - - - 0.0 - - - - - base_xz - passive_block - - - 0 1 0 - - - 0.0 - - - - - - \ No newline at end of file diff --git a/bindings/pydairlib/solvers/BUILD.bazel b/bindings/pydairlib/solvers/BUILD.bazel deleted file mode 100644 index 5fe19d4692..0000000000 --- a/bindings/pydairlib/solvers/BUILD.bazel +++ /dev/null @@ -1,54 +0,0 @@ -# -*- python -*- -load("@drake//tools/install:install.bzl", "install") -load( - "@drake//tools/skylark:pybind.bzl", - "drake_pybind_library", - "get_drake_py_installs", - "get_pybind_package_info", - "pybind_py_library", -) -#load("@rules_python//python:packaging.bzl", "py_package", "py_wheel") - -package(default_visibility = ["//visibility:public"]) - -pybind_py_library( - name = "c3_py", - cc_deps = [ - "//solvers:c3", - "@drake//bindings/pydrake/common:default_scalars_pybind", - "@drake//bindings/pydrake/common:deprecation_pybind", - "@drake//bindings/pydrake/common:sorted_pair_pybind", - ], - cc_so_name = "c3", - cc_srcs = ["c3_py.cc"], - py_deps = [ - ":module_py", - ], - py_imports = ["."], -) - -# This determines how `PYTHONPATH` is configured, and how to install the -# bindings. -PACKAGE_INFO = get_pybind_package_info("//bindings") - -py_library( - name = "module_py", - srcs = [ - "__init__.py", - ], - imports = PACKAGE_INFO.py_imports, - deps = [ - ], -) - -PY_LIBRARIES = [ - ":module_py", - ":c3_py", -] - -# Package roll-up (for Bazel dependencies). -py_library( - name = "pyc3", - imports = PACKAGE_INFO.py_imports, - deps = PY_LIBRARIES, -) diff --git a/bindings/pydairlib/solvers/__init__.py b/bindings/pydairlib/solvers/__init__.py deleted file mode 100644 index 6faf2149f8..0000000000 --- a/bindings/pydairlib/solvers/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# Importing everything in this directory to this package -from . import * -from .c3 import * diff --git a/bindings/pydairlib/solvers/c3_py.cc b/bindings/pydairlib/solvers/c3_py.cc deleted file mode 100644 index 51dc308006..0000000000 --- a/bindings/pydairlib/solvers/c3_py.cc +++ /dev/null @@ -1,129 +0,0 @@ -#include -#include -#include -#include - -#include "solvers/c3_miqp.h" -#include "solvers/lcs.h" -#include "solvers/lcs_factory.h" - -namespace py = pybind11; - -using Eigen::MatrixXd; -using Eigen::VectorXd; -using py::arg; -using std::vector; - -using dairlib::solvers::C3MIQP; -using dairlib::solvers::LCS; - -namespace c3 { -namespace pyc3 { - -PYBIND11_MODULE(c3, m) { - py::class_(m, "LCS") - .def(py::init&, const vector&, - const vector&, const vector&, - const vector&, const vector&, - const vector&, const vector&, double>(), - arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"), - arg("c"), arg("dt")) - .def(py::init(), - arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"), - arg("c"), arg("N"), arg("dt")) - .def_readwrite("A_", &LCS::A_) - .def_readwrite("B_", &LCS::B_) - .def_readwrite("D_", &LCS::D_) - .def_readwrite("d_", &LCS::d_) - .def_readwrite("E_", &LCS::E_) - .def_readwrite("F_", &LCS::F_) - .def_readwrite("H_", &LCS::H_) - .def_readwrite("c_", &LCS::c_) - .def("Simulate", &LCS::Simulate); - - m.def("LinearizePlantToLCS", - &dairlib::solvers::LCSFactory::LinearizePlantToLCS, - py::arg("plant"), - py::arg("context"), - py::arg("plant_ad"), - py::arg("context_ad"), - py::arg("contact_geoms"), - py::arg("mu"), - py::arg("dt"), - py::arg("N"), - py::arg("n_lambda_with_tangential"), - py::arg("num_friction_directions_per_contact"), - py::arg("starting_index_per_contact_in_lambda_t_vector"), - py::arg("contact_model")); - - { - using Enum = dairlib::solvers::ContactModel; - py::enum_ enum_py(m, "ContactModel"); - enum_py // BR - .value("kStewartAndTrinkle", Enum::kStewartAndTrinkle) - .value("kAnitescu", Enum::kAnitescu); - } - - py::class_(m, "CostMatrices") - .def(py::init&, const std::vector&, - const std::vector&, const std::vector&>(), - arg("Q"), arg("R"), arg("G"), arg("U")); - - py::class_(m, "C3MIQP") - .def(py::init&, const C3Options&>(), - arg("LCS"), arg("costs"), arg("x_des"), arg("c3_options")) - .def("Solve", &C3MIQP::Solve, arg("x0"), arg("verbose") = false) - .def("UpdateTarget", &C3MIQP::UpdateTarget, arg("x0")) - .def("UpdateLCS", &C3MIQP::UpdateLCS, arg("lcs")) - .def("ADMMStep", &C3MIQP::ADMMStep, arg("x0"), arg("delta"), arg("w"), - arg("G"), arg("admm_iteration"), arg("verbose") = false) - .def("SolveQP", &C3MIQP::SolveQP, arg("x0"), arg("G"), arg("WD"), arg("delta"), - arg("admm_iteration"), arg("is_final_solve")) - .def("SolveProjection", &C3MIQP::SolveProjection, arg("U"), arg("WZ"), arg("admm_iteration")) - .def("AddLinearConstraint", &C3MIQP::AddLinearConstraint, arg("A"), - arg("lower_bound"), arg("upper_bound"), arg("constraint")) - .def("RemoveConstraints", &C3MIQP::RemoveUserConstraints) - .def("GetFullSolution", &C3MIQP::GetFullSolution) - .def("GetStateSolution", &C3MIQP::GetStateSolution) - .def("GetForceSolution", &C3MIQP::GetForceSolution) - .def("GetInputSolution", &C3MIQP::GetInputSolution) - .def("GetDualDeltaSolution", &C3MIQP::GetDualDeltaSolution) - .def("GetDualWSolution", &C3MIQP::GetDualWSolution); - - py::class_ options(m, "C3Options"); - options.def(py::init<>()) - .def_readwrite("admm_iter", &C3Options::admm_iter) - .def_readwrite("rho", &C3Options::rho) - .def_readwrite("rho_scale", &C3Options::rho_scale) - .def_readwrite("num_threads", &C3Options::num_threads) - .def_readwrite("delta_option", &C3Options::delta_option) - .def_readwrite("contact_model", &C3Options::contact_model) - .def_readwrite("warm_start", &C3Options::warm_start) - .def_readwrite("use_predicted_x0", &C3Options::use_predicted_x0) - .def_readwrite("end_on_qp_step", &C3Options::end_on_qp_step) - .def_readwrite("solve_time_filter_alpha", - &C3Options::solve_time_filter_alpha) - .def_readwrite("publish_frequency", &C3Options::publish_frequency) - .def_readwrite("u_horizontal_limits", &C3Options::u_horizontal_limits) - .def_readwrite("u_vertical_limits", &C3Options::u_vertical_limits) - .def_readwrite("workspace_limits", &C3Options::workspace_limits) - .def_readwrite("workspace_margins", &C3Options::workspace_margins) - .def_readwrite("N", &C3Options::N) - .def_readwrite("gamma", &C3Options::gamma) - .def_readwrite("mu", &C3Options::mu) - .def_readwrite("dt", &C3Options::dt) - .def_readwrite("solve_dt", &C3Options::solve_dt) - .def_readwrite("num_friction_directions", - &C3Options::num_friction_directions) - .def_readwrite("num_contacts", &C3Options::num_contacts) - .def_readwrite("Q", &C3Options::Q) - .def_readwrite("R", &C3Options::R) - .def_readwrite("G", &C3Options::G) - .def_readwrite("U", &C3Options::U); -} - -} // namespace pyc3 -} // namespace c3 diff --git a/examples/franka/BUILD.bazel b/examples/franka/BUILD.bazel deleted file mode 100644 index dd4655ae72..0000000000 --- a/examples/franka/BUILD.bazel +++ /dev/null @@ -1,286 +0,0 @@ -# -*- mode: python -*- -# vi: set ft=python : - -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "urdfs", - data = glob([ - "urdf/**", - ]), -) - -cc_binary( - name = "full_diagram", - srcs = ["full_diagram.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//examples/franka/diagrams:franka_c3_controller_diagram", - "//examples/franka/diagrams:franka_osc_controller_diagram", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "@drake//:drake_shared_library", - ], -) - -cc_binary( - name = "franka_sim", - srcs = ["franka_sim.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//examples/franka/systems:franka_systems", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_osc_controller", - srcs = ["franka_osc_controller.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//examples/franka/systems:franka_systems", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers/osc:operational_space_control", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:lcm_trajectory_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_c3_controller", - srcs = ["franka_c3_controller.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//examples/franka/systems:franka_systems", - "//systems:franka_kinematics", - "//systems/senders:c3_state_sender", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers:c3_controller", - "//systems/controllers:lcs_factory_system", - "//systems/controllers/osc:operational_space_control", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:c3_output_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_c3_controller_two_objects", - srcs = ["franka_c3_controller_two_objects.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//examples/franka/systems:franka_systems", - "//systems:franka_kinematics", - "//systems/senders:c3_state_sender", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers:c3_controller", - "//systems/controllers:lcs_factory_system", - "//systems/controllers/osc:operational_space_control", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:c3_output_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "forward_kinematics_for_lcs", - srcs = ["forward_kinematics_for_lcs.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//examples/franka/systems:franka_systems", - "//systems:franka_kinematics", - "//systems/senders:c3_state_sender", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers:c3_controller", - "//systems/controllers:lcs_factory_system", - "//systems/controllers/osc:operational_space_control", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:c3_output_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_visualizer", - srcs = ["franka_visualizer.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//multibody:utils", - "//multibody:visualization_utils", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/primitives", - "//systems/trajectory_optimization:lcm_trajectory_systems", - "//systems/visualization:lcm_visualization_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_bridge_driver_out", - srcs = ["franka_bridge_driver_out.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//systems:franka_state_translator", - "//multibody:utils", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/framework:lcm_driven_loop", - "//systems/primitives", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_bridge_driver_in", - srcs = ["franka_bridge_driver_in.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - deps = [ - ":parameters", - "//common", - "//systems:franka_state_translator", - "//multibody:utils", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/framework:lcm_driven_loop", - "//systems/primitives", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_binary( - name = "franka_ros_lcm_bridge", - srcs = ["franka_ros_lcm_bridge.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - tags = ["ros"], - deps = [ - ":parameters", - "//common", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/primitives", - "//systems/ros:franka_ros_channels", - "//systems/ros:franka_ros_lcm_conversions", - "//systems/ros:ros_pubsub_systems", - "@drake//:drake_shared_library", - "@gflags", - "@ros", - ], -) - -cc_binary( - name = "franka_lcm_ros_bridge", - srcs = ["franka_lcm_ros_bridge.cc"], - data = [ - ":urdfs", - "@drake_models//:franka_description", - ], - tags = ["ros"], - deps = [ - ":parameters", - "//common", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/framework:lcm_driven_loop", - "//systems/ros:franka_ros_channels", - "//systems/ros:franka_ros_lcm_conversions", - "//systems/ros:ros_pubsub_systems", - "@drake//:drake_shared_library", - "@gflags", - "@ros", - ], -) - -cc_library( - name = "parameters", - hdrs = [ - "parameters/franka_c3_controller_params.h", - "parameters/franka_c3_scene_params.h", - "parameters/franka_lcm_channels.h", - "parameters/franka_osc_controller_params.h", - "parameters/franka_sim_params.h", - "parameters/franka_sim_scene_params.h", - ], - data = glob([ - "parameters/*.yaml", - ]), - deps = [ - "@drake//:drake_shared_library", - "//common/parameters:franka_drake_lcm_driver_channels", - ], -) diff --git a/examples/franka/README.md b/examples/franka/README.md deleted file mode 100644 index 989a8dc943..0000000000 --- a/examples/franka/README.md +++ /dev/null @@ -1,77 +0,0 @@ -## Experiment Instructions - -Central repo for C3 and its examples, including: -- https://arxiv.org/abs/2405.08731 - -This branch/repo is being constantly updated so examples may be unstable. - -## Simulated Robot - -1. Start the procman script containing a list of relevant processes -``` -bot-procman-sheriff -l franka_sim.pmd -``` - -2. In the procman window, start the operator processes (meshcat visualizer) using the script `script:start_operator_commands`. Scripts are located in the top bar of the procman window. - -3. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000` - -4. The examples with the C3 controller can be run using the script `script:c3_mpc`. Note, the task can be changed by changing the `scene_index` in the parameters. More details in [changing the scene](#changing-the-scene). This script spawns three processes: - - `bazel-bin/examples/franka/franka_sim`: Simulated environment which takes in torques commands from `franka_osc` and publishes the state of the system via LCM on various channels. - - `bazel-bin/examples/franka/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC - - `bazel-bin/examples/franka/franka_c3_controller`: Contact Implicit MPC controller that takes in the state of the system and publishes end effector trajectories to be tracked by the OSC. - -5. The simulator and controller can be stopped using the script `script:stop_controllers_and_simulators`. -6. A comparison using the sampling based MPC controllers from the [MJMPC controllers](https://github.com/google-deepmind/mujoco_mpc) can be run using `script:mjmpc_with_drake_sim`. This extracts out just the controller portion of the MJMPC code base and runs in on the identical task (scene 1) in the Drake simulator. Instructions to build and configure the standalone MJMPC controllers are a WIP. - -## Physical Robot - -Hardware instructions updated for Ubuntu 22.04. We are no longer using ROS or ROS2 and instead relying on [drake-franka-driver](https://github.com/RobotLocomotion/drake-franka-driver), which works via LCM. Much thanks to the Drake developers who provided this! - -### Installing `drake-franka-driver` - -``` -git clone https://github.com/RobotLocomotion/drake-franka-driver -cd drake-franka-driver -bazel build ... -``` - - -### Running Experiments - -1. Start the procman script containing a list of relevant processes. The primary differences from`franka_sim.pmd` script are the lcm_channels and the drake-franka-driver and corresponding translators to communicate with the Franka via LCM. - - - In the root of dairlib: ``` bot-procman-sheriff -l examples/franka/franka_hardware.pmd ``` - - In the root of drake-franka-driver: ```bot-procman-deputy franka_control``` - -2. In the procman window, start the operator processes (meshcat visualizer and xbox controller) using the script `script:start_operator_commands`. Scripts are located in the top bar of the procman window. - -3. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000` - -4. The processes, except the C3 controller, can be run using the script `script:start_experiment`. This spawns the following processes: - - `start_logging.py`: Starts a lcm-logger with an automatic naming convention for the log number. - - `record_video.py`: Streams all available webcams to a .mp4 file corresponding to the log number. - - `torque_driver`: `drake-franka-driver` in torque control mode. - - `franka_driver_`(in/out): communicates with `drake-franka-driver` to receive/publish franka state information and torque commands. This is just a translator between Drake's Franka Panda specific lcm messages and the standardized robot commands that we use. - - `bazel-bin/examples/franka/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC - - -5. For safety, start the C3 controller separately after verifying the rest of the processes have started successfully, by manually starting the `franka_c3` process. -6. Using the xbox controller, switch from tracking the teleop commands to the MPC plan by pressing "A". -7. Stop the experiment using `script:stop_experiment`. This also stops logging and recording. - - -## Changing the scene - -We currently have environment descriptions and gains for the following scenes: - -The scene can be changed by updating the `scene_index` parameter in BOTH `franka_sim_params.yaml` and `franka_c3_controller_params.yaml`. -The visualizer process must be restarted if changing the scene. - -| Scene Index | Description | -|-------------|-----------------------------------------------------------------------------------------------------------------------------------------------| -| 0 | No fixed environment features. For testing controlled sliding purely between the end effector and tray + optional objects. Gains still a WIP. | -| 1 | Primary RSS paper example with supports | -| 2 | Variation on RSS paper example with rotated supports | -| 3 | Additional rotating with wall task for RSS paper | - diff --git a/examples/franka/diagrams/BUILD.bazel b/examples/franka/diagrams/BUILD.bazel deleted file mode 100644 index bedf79b67d..0000000000 --- a/examples/franka/diagrams/BUILD.bazel +++ /dev/null @@ -1,59 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "franka_c3_controller_diagram", - srcs = ["franka_c3_controller_diagram.cc"], - hdrs = ["franka_c3_controller_diagram.h"], - deps = [ - "//common", - "//examples/franka:parameters", - "//examples/franka/systems:franka_systems", - "//systems/senders:c3_state_sender", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers:c3_controller", - "//systems/controllers:lcs_factory_system", - "//systems/controllers/osc:operational_space_control", - "//systems/framework:lcm_driven_loop", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:c3_output_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_library( - name = "franka_osc_controller_diagram", - srcs = ["franka_osc_controller_diagram.cc"], - hdrs = ["franka_osc_controller_diagram.h"], - deps = [ - "//common", - "//examples/franka:parameters", - "//examples/franka/systems:franka_systems", - "//lcm:lcm_trajectory_saver", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "//systems/controllers", - "//systems/controllers/osc:operational_space_control", - "//systems/primitives:radio_parser", - "//systems/trajectory_optimization:lcm_trajectory_systems", - "@drake//:drake_shared_library", - "@gflags", - ], -) - -cc_library( - name = "franka_sim_diagram", - srcs = ["franka_sim_diagram.cc"], - hdrs = ["franka_sim_diagram.h"], - deps = [ - "//common", - "//examples/franka:parameters", - "//systems:robot_lcm_systems", - "//systems:system_utils", - "@drake//:drake_shared_library", - "@gflags", - ], -) diff --git a/examples/franka/diagrams/franka_c3_controller_diagram.cc b/examples/franka/diagrams/franka_c3_controller_diagram.cc deleted file mode 100644 index e49a5a91df..0000000000 --- a/examples/franka/diagrams/franka_c3_controller_diagram.cc +++ /dev/null @@ -1,339 +0,0 @@ -// -// Created by yangwill on 2/19/24. -// - -#include "examples/franka/diagrams/franka_c3_controller_diagram.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "examples/franka/parameters/franka_c3_controller_params.h" -#include "examples/franka/parameters/franka_c3_scene_params.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "systems/senders/c3_state_sender.h" -#include "examples/franka/systems/c3_trajectory_generator.h" -#include "systems/franka_kinematics.h" -#include "examples/franka/systems/plate_balancing_target.h" -#include "multibody/multibody_utils.h" -#include "solvers/lcs_factory.h" -#include "systems/controllers/c3/lcs_factory_system.h" -#include "systems/controllers/c3/c3_controller.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/c3_output_systems.h" - -namespace dairlib { - -namespace examples { -namespace controllers { - -using dairlib::solvers::LCSFactory; -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::math::RigidTransform; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; - -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; -using std::string; - -FrankaC3ControllerDiagram::FrankaC3ControllerDiagram( - const string& controller_settings, const C3Options c3_options, const string& lcm_channels, - drake::lcm::DrakeLcm* lcm, bool publish_c3_debug) { - this->set_name("FrankaC3Controller"); - DiagramBuilder builder; - - FrankaC3ControllerParams controller_params = - drake::yaml::LoadYamlFile(controller_settings); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(lcm_channels); - FrankaC3SceneParams scene_params = - drake::yaml::LoadYamlFile( - controller_params.c3_scene_file[controller_params.scene_index]); -// C3Options c3_options = drake::yaml::LoadYamlFile( -// controller_params.c3_options_file[controller_params.scene_index]); - drake::solvers::SolverOptions solver_options = - drake::yaml::LoadYamlFile( - FindResourceOrThrow(controller_params.osqp_settings_file)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - - plant_franka_ = new drake::multibody::MultibodyPlant(0.0); - Parser parser_franka(plant_franka_, nullptr); - parser_franka.AddModelsFromUrl(scene_params.franka_model); - drake::multibody::ModelInstanceIndex end_effector_index = - parser_franka.AddModels( - FindResourceOrThrow(scene_params.end_effector_model))[0]; - - RigidTransform X_WI = RigidTransform::Identity(); - plant_franka_->WeldFrames(plant_franka_->world_frame(), - plant_franka_->GetFrameByName("panda_link0"), X_WI); - - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - scene_params.tool_attachment_frame); - plant_franka_->WeldFrames( - plant_franka_->GetFrameByName("panda_link7"), - plant_franka_->GetFrameByName("plate", end_effector_index), T_EE_W); - - plant_franka_->Finalize(); - plant_franka_context_ = plant_franka_->CreateDefaultContext(); - - /// - plant_tray_ = new drake::multibody::MultibodyPlant(0.0); - Parser parser_tray(plant_tray_, nullptr); - parser_tray.AddModels(scene_params.object_models[0]); - plant_tray_->Finalize(); - plant_tray_context_ = plant_tray_->CreateDefaultContext(); - - drake::planning::RobotDiagramBuilder lcs_diagram_builder; - lcs_diagram_builder.parser().SetAutoRenaming(true); - lcs_diagram_builder.parser().AddModels(scene_params.end_effector_lcs_model); - - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = lcs_diagram_builder.parser().AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - lcs_diagram_builder.plant().WeldFrames( - lcs_diagram_builder.plant().world_frame(), - lcs_diagram_builder.plant().GetFrameByName("base", environment_model_indices[i]), - T_E_W); - } - for (int i = 0; i < scene_params.object_models.size(); ++i) { - lcs_diagram_builder.parser().AddModels(scene_params.object_models[i]); - } - lcs_diagram_builder.plant().WeldFrames( - lcs_diagram_builder.plant().world_frame(), - lcs_diagram_builder.plant().GetFrameByName("base_link"), X_WI); - lcs_diagram_builder.plant().Finalize(); - robot_diagram_for_lcs_ = lcs_diagram_builder.Build(); - plant_for_lcs_autodiff_ = drake::systems::System::ToAutoDiffXd( - robot_diagram_for_lcs_->plant()); - - robot_diagram_root_context_ = robot_diagram_for_lcs_->CreateDefaultContext(); - plant_for_lcs_autodiff_context_ = - plant_for_lcs_autodiff_->CreateDefaultContext(); - - /// - std::vector end_effector_contact_points = - robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( - robot_diagram_for_lcs_->plant().GetBodyByName("plate")); - for (int i = 0; i < environment_model_indices.size(); ++i) { - std::vector - environment_support_contact_points = - robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( - robot_diagram_for_lcs_->plant().GetBodyByName("base", - environment_model_indices[i])); - end_effector_contact_points.insert( - end_effector_contact_points.end(), - environment_support_contact_points.begin(), - environment_support_contact_points.end()); - } - std::vector tray_geoms = - robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( - robot_diagram_for_lcs_->plant().GetBodyByName("tray")); - std::unordered_map> - contact_geoms; - contact_geoms["PLATE"] = end_effector_contact_points; - contact_geoms["TRAY"] = tray_geoms; - - std::vector> contact_pairs; - for (auto geom_id : contact_geoms["PLATE"]) { - contact_pairs.emplace_back(geom_id, contact_geoms["TRAY"][0]); - } - - auto franka_state_receiver = - builder.AddSystem(*plant_franka_); - auto tray_state_receiver = - builder.AddSystem(*plant_tray_); - auto reduced_order_model_receiver = - builder.AddSystem( - *plant_franka_, plant_franka_context_.get(), *plant_tray_, - plant_tray_context_.get(), scene_params.end_effector_name, - "tray", controller_params.include_end_effector_orientation); - - auto plate_balancing_target = - builder.AddSystem( - *plant_tray_, scene_params.end_effector_thickness, - controller_params.near_target_threshold); - plate_balancing_target->SetRemoteControlParameters( - controller_params.first_target[controller_params.scene_index], - controller_params.second_target[controller_params.scene_index], - controller_params.third_target[controller_params.scene_index], - controller_params.x_scale, controller_params.y_scale, - controller_params.z_scale); - std::vector input_sizes = {3, 7, 3, 6}; - auto target_state_mux = - builder.AddSystem(input_sizes); - auto end_effector_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(3)); - auto tray_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(6)); - builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), - target_state_mux->get_input_port(0)); - builder.Connect(plate_balancing_target->get_output_port_tray_target(), - target_state_mux->get_input_port(1)); - builder.Connect(end_effector_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(2)); - builder.Connect(tray_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(3)); - auto lcs_factory = builder.AddSystem( - robot_diagram_for_lcs_->plant(), - robot_diagram_for_lcs_->mutable_plant_context( - robot_diagram_root_context_.get()), - *plant_for_lcs_autodiff_, *plant_for_lcs_autodiff_context_, contact_pairs, - c3_options); - auto controller = builder.AddSystem( - robot_diagram_for_lcs_->plant(), c3_options); - auto c3_trajectory_generator = - builder.AddSystem( - robot_diagram_for_lcs_->plant(), c3_options); -// auto placeholder_trajectory = lcmt_timestamped_saved_traj(); - auto placeholder_solution = C3Output::C3Solution(); - placeholder_solution.x_sol_ = Eigen::MatrixXf::Zero(c3_options.g_x.size(), c3_options.N); - placeholder_solution.lambda_sol_ = Eigen::MatrixXf::Zero(c3_options.g_lambda.size(), c3_options.N); - placeholder_solution.u_sol_ = Eigen::MatrixXf::Zero(c3_options.g_u.size(), c3_options.N); - placeholder_solution.time_vector_ = Eigen::VectorXf::LinSpaced(c3_options.N, 0, 1); - auto discrete_time_delay = builder.AddSystem(1 / c3_options.publish_frequency, - 2, - drake::Value(placeholder_solution)); - std::vector state_names = { - "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", - "tray_qx", "tray_qy", "tray_qz", "tray_x", - "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", - "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", - "tray_vz", "tray_vz", "tray_vz", - }; - auto c3_state_sender = - builder.AddSystem(3 + 7 + 3 + 6, state_names); - c3_trajectory_generator->SetPublishEndEffectorOrientation( - controller_params.include_end_effector_orientation); - auto c3_output_sender = builder.AddSystem(); - auto passthrough = builder.AddSystem>(18); - - controller->SetOsqpSolverOptions(solver_options); - - // publisher connections - DRAKE_DEMAND(c3_options.publish_frequency > 0); - builder.Connect(controller->get_output_port_c3_solution(), - discrete_time_delay->get_input_port()); - - - builder.Connect(franka_state_receiver->get_output_port(), - reduced_order_model_receiver->get_input_port_franka_state()); - builder.Connect(target_state_mux->get_output_port(), - controller->get_input_port_target()); - builder.Connect(lcs_factory->get_output_port_lcs(), - controller->get_input_port_lcs()); - builder.Connect(tray_state_receiver->get_output_port(), - *(reduced_order_model_receiver->get_input_ports_object_state()[0])); - builder.Connect(tray_state_receiver->get_output_port(), - plate_balancing_target->get_input_port_tray_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - controller->get_input_port_lcs_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - lcs_factory->get_input_port_lcs_state()); - builder.Connect(passthrough->get_output_port(), - plate_balancing_target->get_input_port_radio()); - builder.Connect(discrete_time_delay->get_output_port(), - c3_trajectory_generator->get_input_port_c3_solution()); - builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), - c3_output_sender->get_input_port_lcs_contact_info()); - - builder.Connect(target_state_mux->get_output_port(), - c3_state_sender->get_input_port_target_state()); - builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), - c3_state_sender->get_input_port_actual_state()); - - builder.Connect(discrete_time_delay->get_output_port(), - c3_output_sender->get_input_port_c3_solution()); - builder.Connect(controller->get_output_port_c3_intermediates(), - c3_output_sender->get_input_port_c3_intermediates()); - - - if (publish_c3_debug){ - auto actor_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_actor_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto object_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_object_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_output_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_debug_output_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_target_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_target_state_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_actual_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_actual_state_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_forces_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_force_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), - actor_trajectory_sender->get_input_port()); - builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), - object_trajectory_sender->get_input_port()); - builder.Connect(c3_state_sender->get_output_port_target_c3_state(), - c3_target_state_publisher->get_input_port()); - builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), - c3_actual_state_publisher->get_input_port()); - builder.Connect(c3_output_sender->get_output_port_c3_debug(), - c3_output_publisher->get_input_port()); - builder.Connect(c3_output_sender->get_output_port_c3_force(), - c3_forces_publisher->get_input_port()); - } - - - - // Publisher connections - franka_state_port_ = builder.ExportInput(franka_state_receiver->get_input_port(), - "franka_state: lcmt_robot_output"); - tray_state_port_ = builder.ExportInput(tray_state_receiver->get_input_port(), - "tray_state: lcmt_object_state"); - radio_port_ = builder.ExportInput(passthrough->get_input_port(), "raw_radio"); - mpc_plan_port_ = builder.ExportOutput( - c3_trajectory_generator->get_output_port_actor_trajectory(), - "actor_trajectory"); - - builder.BuildInto(this); - this->set_name("FrankaC3Controller"); -} - -} // namespace controllers -} // namespace examples -} // namespace dairlib diff --git a/examples/franka/diagrams/franka_c3_controller_diagram.h b/examples/franka/diagrams/franka_c3_controller_diagram.h deleted file mode 100644 index 4e0abb92c4..0000000000 --- a/examples/franka/diagrams/franka_c3_controller_diagram.h +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include "solvers/c3_options.h" - -namespace dairlib { -namespace examples { -namespace controllers { - -class FrankaC3ControllerDiagram : public drake::systems::Diagram { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaC3ControllerDiagram) - - /// @param[in] osc_gains_filename filepath containing the osc_running_gains. - /// @param[in] osqp_settings filepath containing the osqp settings. - FrankaC3ControllerDiagram(const std::string& controller_settings, const C3Options c3_options, - const std::string& lcm_channels, - drake::lcm::DrakeLcm* lcm, bool publish_c3_debug = false); - - /// @return the input port for the plant state. - const drake::systems::InputPort& get_input_port_robot_state() const { - return this->get_input_port(franka_state_port_); - } - - /// @return the input port for the plant state. - const drake::systems::InputPort& get_input_port_object_state() const { - return this->get_input_port(tray_state_port_); - } - - /// @return the input port for the cassie_out struct (containing radio - /// commands). - const drake::systems::InputPort& get_input_port_radio() const { - return this->get_input_port(radio_port_); - } - - /// @return the output port for the lcmt_robot_input message. - const drake::systems::OutputPort& get_output_port_mpc_plan() const { - return this->get_output_port(mpc_plan_port_); - } - - private: - drake::multibody::MultibodyPlant* plant_franka_; - drake::multibody::MultibodyPlant* plant_tray_; - - // Storage for the diagram and its plant and scene graph. - // After Build(), the `builder_` is set to nullptr. - std::unique_ptr> robot_diagram_for_lcs_; - - std::unique_ptr> robot_diagram_root_context_; - std::unique_ptr> plant_for_lcs_diagram_context_; - std::unique_ptr> plant_for_lcs_autodiff_; - std::unique_ptr> plant_franka_context_; - std::unique_ptr> plant_tray_context_; - std::unique_ptr> plant_for_lcs_autodiff_context_; - - drake::systems::InputPortIndex franka_state_port_; - drake::systems::InputPortIndex tray_state_port_; - drake::systems::InputPortIndex radio_port_; - drake::systems::OutputPortIndex mpc_plan_port_; -}; - -} // namespace controllers -} // namespace examples -} // namespace dairlib diff --git a/examples/franka/diagrams/franka_osc_controller_diagram.cc b/examples/franka/diagrams/franka_osc_controller_diagram.cc deleted file mode 100644 index 21b8967d33..0000000000 --- a/examples/franka/diagrams/franka_osc_controller_diagram.cc +++ /dev/null @@ -1,267 +0,0 @@ -// -// Created by yangwill on 2/19/24. -// - -#include "examples/franka/diagrams/franka_osc_controller_diagram.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_osc_controller_params.h" -#include "systems/controllers/osc/end_effector_force.h" -#include "systems/controllers/osc/end_effector_orientation.h" -#include "systems/controllers/osc/end_effector_position.h" -#include "lcm/lcm_trajectory.h" -#include "multibody/multibody_utils.h" -#include "systems/controllers/gravity_compensator.h" -#include "systems/controllers/osc/operational_space_control.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/lcm_trajectory_systems.h" - -namespace dairlib { - -namespace examples { -namespace controllers { - -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; - -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; -using std::string; - -FrankaOSCControllerDiagram::FrankaOSCControllerDiagram( - const string& controller_settings, const string& lcm_channels, - drake::lcm::DrakeLcm* lcm) { - DiagramBuilder builder; - - // load parameters - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaControllerParams controller_params = - drake::yaml::LoadYamlFile(controller_settings); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(lcm_channels); - OSCGains gains = drake::yaml::LoadYamlFile( - FindResourceOrThrow(controller_settings), {}, {}, yaml_options); - drake::solvers::SolverOptions solver_options = - drake::yaml::LoadYamlFile( - FindResourceOrThrow( - "examples/franka/parameters/franka_osc_qp_settings.yaml")) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - - plant_ = new drake::multibody::MultibodyPlant(0.0); - Parser parser(plant_, nullptr); - parser.AddModelsFromUrl(controller_params.franka_model); - - RigidTransform X_WI = RigidTransform::Identity(); - plant_->WeldFrames(plant_->world_frame(), - plant_->GetFrameByName("panda_link0"), X_WI); - - if (!controller_params.end_effector_name.empty()) { - drake::multibody::ModelInstanceIndex end_effector_index = parser.AddModels( - FindResourceOrThrow(controller_params.end_effector_model))[0]; - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - controller_params.tool_attachment_frame); - plant_->WeldFrames( - plant_->GetFrameByName("panda_link7"), - plant_->GetFrameByName(controller_params.end_effector_name, - end_effector_index), - T_EE_W); - } else { - std::cout << "OSC plant has been constructed with no end effector." - << std::endl; - } - - plant_->Finalize(); - plant_context_ = plant_->CreateDefaultContext(); - - auto state_receiver = - builder.AddSystem(*plant_); - auto end_effector_position_receiver = - builder.AddSystem( - "end_effector_position_target"); - auto end_effector_force_receiver = - builder.AddSystem( - "end_effector_force_target"); - auto end_effector_orientation_receiver = - builder.AddSystem( - "end_effector_orientation_target"); - auto franka_command_sender = - builder.AddSystem(*plant_); - auto osc_command_sender = - builder.AddSystem(*plant_); - auto end_effector_trajectory = - builder.AddSystem(*plant_, - plant_context_.get(), controller_params.neutral_position, false, - controller_params.end_effector_name); - auto passthrough = builder.AddSystem>(18); - end_effector_trajectory->SetRemoteControlParameters( - controller_params.neutral_position, controller_params.x_scale, - controller_params.y_scale, controller_params.z_scale); - auto end_effector_orientation_trajectory = - builder.AddSystem(); - end_effector_orientation_trajectory->SetTrackOrientation( - controller_params.track_end_effector_orientation); - auto end_effector_force_trajectory = - builder.AddSystem(); - auto osc = builder.AddSystem( - *plant_, *plant_, plant_context_.get(), plant_context_.get(), false); - if (controller_params.publish_debug_info) { - auto franka_command_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_input_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto osc_command_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.osc_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto osc_debug_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.osc_debug_channel, lcm, - TriggerTypeSet({TriggerType::kForced}))); - builder.Connect(osc->get_output_port_osc_debug(), - osc_debug_pub->get_input_port()); - builder.Connect(franka_command_sender->get_output_port(), - franka_command_pub->get_input_port()); - builder.Connect(osc_command_sender->get_output_port(), - osc_command_pub->get_input_port()); - } - - auto end_effector_position_tracking_data = - std::make_unique( - "end_effector_target", controller_params.K_p_end_effector, - controller_params.K_d_end_effector, controller_params.W_end_effector, - *plant_, *plant_); - end_effector_position_tracking_data->AddPointToTrack( - controller_params.end_effector_name); - const VectorXd& end_effector_acceleration_limits = - controller_params.end_effector_acceleration * Vector3d::Ones(); - end_effector_position_tracking_data->SetCmdAccelerationBounds( - -end_effector_acceleration_limits, end_effector_acceleration_limits); - auto mid_link_position_tracking_data_for_rel = - std::make_unique( - "panda_joint2_target", controller_params.K_p_mid_link, - controller_params.K_d_mid_link, controller_params.W_mid_link, *plant_, - *plant_); - mid_link_position_tracking_data_for_rel->AddJointToTrack("panda_joint2", - "panda_joint2dot"); - - auto end_effector_force_tracking_data = - std::make_unique( - "end_effector_force", controller_params.W_ee_lambda, *plant_, *plant_, - controller_params.end_effector_name, Vector3d::Zero()); - - auto end_effector_orientation_tracking_data = - std::make_unique( - "end_effector_orientation_target", - controller_params.K_p_end_effector_rot, - controller_params.K_d_end_effector_rot, - controller_params.W_end_effector_rot, *plant_, *plant_); - end_effector_orientation_tracking_data->AddFrameToTrack( - controller_params.end_effector_name); - Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4); - orientation_target(0) = 1; - osc->AddTrackingData(std::move(end_effector_position_tracking_data)); - osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel), - 1.6 * VectorXd::Ones(1)); - osc->AddTrackingData(std::move(end_effector_orientation_tracking_data)); - osc->AddForceTrackingData(std::move(end_effector_force_tracking_data)); - osc->SetAccelerationCostWeights(gains.W_acceleration); - osc->SetInputCostWeights(gains.W_input_regularization); - osc->SetInputSmoothingCostWeights(gains.W_input_smoothing_regularization); - osc->SetAccelerationConstraints( - controller_params.enforce_acceleration_constraints); - - osc->SetContactFriction(controller_params.mu); - osc->SetOsqpSolverOptions(solver_options); - - osc->Build(); - - if (controller_params.cancel_gravity_compensation) { - auto gravity_compensator = - builder.AddSystem(*plant_, - *plant_context_); - builder.Connect(osc->get_output_port_osc_command(), - gravity_compensator->get_input_port()); - builder.Connect(gravity_compensator->get_output_port(), - franka_command_sender->get_input_port()); - } else { - DRAKE_DEMAND(lcm_channels == - "examples/franka/parameters/lcm_channels_simulation.yaml"); - builder.Connect(osc->get_output_port_osc_command(), - franka_command_sender->get_input_port(0)); - } - - builder.Connect(passthrough->get_output_port(), - end_effector_trajectory->get_input_port_radio()); - builder.Connect(passthrough->get_output_port(), - end_effector_orientation_trajectory->get_input_port_radio()); - builder.Connect(passthrough->get_output_port(), - end_effector_force_trajectory->get_input_port_radio()); - - builder.Connect(osc->get_output_port_osc_command(), - osc_command_sender->get_input_port(0)); - builder.Connect(state_receiver->get_output_port(0), - osc->get_input_port_robot_output()); - builder.Connect(end_effector_position_receiver->get_output_port(0), - end_effector_trajectory->get_input_port_trajectory()); - builder.Connect( - end_effector_orientation_receiver->get_output_port(0), - end_effector_orientation_trajectory->get_input_port_trajectory()); - builder.Connect(end_effector_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_target")); - builder.Connect( - end_effector_orientation_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_orientation_target")); - builder.Connect(end_effector_force_receiver->get_output_port(0), - end_effector_force_trajectory->get_input_port_trajectory()); - builder.Connect(end_effector_force_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_force")); - - // Publisher connections - franka_state_port_ = builder.ExportInput(state_receiver->get_input_port(), - "lcmt_robot_output"); - end_effector_position_port_ = - builder.ExportInput(end_effector_position_receiver->get_input_port(), - "end_effector_position: lcmt_timestamped_trajectory"); - end_effector_orientation_port_ = builder.ExportInput( - end_effector_orientation_receiver->get_input_port(), - "end_effector_orientation: lcmt_timestamped_trajectory"); - end_effector_force_port_ = - builder.ExportInput(end_effector_force_receiver->get_input_port(), - "end_effector_force: lcmt_timestamped_trajectory"); - radio_port_ = builder.ExportInput(passthrough->get_input_port(), "raw_radio"); - builder.ExportOutput(osc->get_output_port_osc_command(), "robot_input"); - - builder.BuildInto(this); - this->set_name("FrankaOSCController"); -} - -} // namespace controllers -} // namespace examples -} // namespace dairlib diff --git a/examples/franka/diagrams/franka_osc_controller_diagram.h b/examples/franka/diagrams/franka_osc_controller_diagram.h deleted file mode 100644 index 4c5a48e058..0000000000 --- a/examples/franka/diagrams/franka_osc_controller_diagram.h +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "systems/controllers/osc/joint_space_tracking_data.h" -#include "systems/controllers/osc/relative_translation_tracking_data.h" -#include "systems/controllers/osc/rot_space_tracking_data.h" -#include "systems/controllers/osc/external_force_tracking_data.h" -#include "systems/controllers/osc/trans_space_tracking_data.h" - -namespace dairlib { -namespace examples { -namespace controllers { - -using systems::controllers::JointSpaceTrackingData; -using systems::controllers::RotTaskSpaceTrackingData; -using systems::controllers::TransTaskSpaceTrackingData; -using systems::controllers::ExternalForceTrackingData; - -class FrankaOSCControllerDiagram : public drake::systems::Diagram { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaOSCControllerDiagram) - - /// @param[in] osc_gains_filename filepath containing the osc_running_gains. - /// @param[in] osqp_settings filepath containing the osqp settings. - FrankaOSCControllerDiagram(const std::string& controller_settings, - const std::string& lcm_channels, - drake::lcm::DrakeLcm* lcm); - - /// @return the input port for the plant state. - const drake::systems::InputPort& get_input_port_robot_state() const { - return this->get_input_port(franka_state_port_); - } - - const drake::systems::InputPort& get_input_port_end_effector_position() const { - return this->get_input_port(end_effector_position_port_); - } - - const drake::systems::InputPort& get_input_port_end_effector_orientation() const { - return this->get_input_port(end_effector_orientation_port_); - } - - const drake::systems::InputPort& get_input_port_end_effector_force() const { - return this->get_input_port(end_effector_force_port_); - } - - const drake::systems::InputPort& get_input_port_radio() const { - return this->get_input_port(radio_port_); - } - - /// @return the output port for the lcmt_robot_input message. - const drake::systems::OutputPort& get_output_port_robot_input() const { - return this->get_output_port(robot_input_port_); - } - - private: - drake::multibody::MultibodyPlant* plant_; - std::unique_ptr> plant_context_; - - drake::systems::InputPortIndex franka_state_port_; - drake::systems::InputPortIndex end_effector_position_port_; - drake::systems::InputPortIndex end_effector_orientation_port_; - drake::systems::InputPortIndex end_effector_force_port_; - drake::systems::InputPortIndex radio_port_; - const drake::systems::OutputPortIndex robot_input_port_ = - drake::systems::OutputPortIndex(0); -}; - -} // namespace controllers -} // namespace examples -} // namespace dairlib diff --git a/examples/franka/diagrams/franka_sim_diagram.cc b/examples/franka/diagrams/franka_sim_diagram.cc deleted file mode 100644 index 584acef837..0000000000 --- a/examples/franka/diagrams/franka_sim_diagram.cc +++ /dev/null @@ -1,141 +0,0 @@ - -#include "franka_sim_diagram.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "common/find_resource.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "examples/franka/parameters/franka_sim_scene_params.h" -#include "systems/robot_lcm_systems.h" - -namespace dairlib { -namespace examples { -using drake::systems::lcm::LcmPublisherSystem; - -using drake::geometry::GeometrySet; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::systems::DiagramBuilder; - -FrankaSimDiagram::FrankaSimDiagram(std::unique_ptr> plant, - drake::lcm::DrakeLcm* lcm) { - // load parameters - DiagramBuilder builder; - scene_graph_ = builder.AddSystem(); - scene_graph_->set_name("scene_graph"); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile("examples/franka/parameters/lcm_channels_simulation.yaml"); - FrankaSimSceneParams scene_params = - drake::yaml::LoadYamlFile( - sim_params.sim_scene_file[sim_params.scene_index]); - /// Sim Start - plant_ = builder.AddSystem(std::move(plant)); - - auto parser = drake::multibody::Parser(plant_, scene_graph_); -// lcs_diagram_builder.parser() lcs_diagram_builder.parser()(&plant); - parser.SetAutoRenaming(true); - drake::multibody::ModelInstanceIndex franka_index = - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - drake::multibody::ModelInstanceIndex c3_end_effector_index = - parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; - drake::multibody::ModelInstanceIndex tray_index = - parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; - - RigidTransform T_X_W = RigidTransform( - drake::math::RotationMatrix(), Eigen::VectorXd::Zero(3)); - RigidTransform T_EE_W = RigidTransform( - drake::math::RotationMatrix(), sim_params.tool_attachment_frame); - plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("panda_link0"), - T_X_W); - plant_->WeldFrames(plant_->GetFrameByName("panda_link7"), - plant_->GetFrameByName("plate", c3_end_effector_index), - T_EE_W); - - // we WANT to model collisions between link5 and the supports - const drake::geometry::GeometrySet& franka_geom_set = - plant->CollectRegisteredGeometries({&plant->GetBodyByName("panda_link0"), - &plant->GetBodyByName("panda_link1"), - &plant->GetBodyByName("panda_link2"), - &plant->GetBodyByName("panda_link3"), - &plant->GetBodyByName("panda_link4")}); - drake::geometry::GeometrySet support_geom_set; - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant->WeldFrames( - plant->world_frame(), - plant->GetFrameByName("base", environment_model_indices[i]), - T_E_W); - support_geom_set.Add(plant->GetCollisionGeometriesForBody(plant->GetBodyByName("base", - environment_model_indices[i]))); - } - plant->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"supports", support_geom_set}, {"franka", franka_geom_set}); - - const drake::geometry::GeometrySet& paddle_geom_set = - plant_->CollectRegisteredGeometries({ - &plant_->GetBodyByName("panda_link2"), - &plant_->GetBodyByName("panda_link3"), - &plant_->GetBodyByName("panda_link4"), - &plant_->GetBodyByName("panda_link5"), - &plant_->GetBodyByName("panda_link6"), - &plant_->GetBodyByName("panda_link8"), - }); - auto tray_collision_set = GeometrySet( - plant_->GetCollisionGeometriesForBody(plant_->GetBodyByName("tray"))); - plant_->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"paddle", paddle_geom_set}, {"tray", tray_collision_set}); - - plant_->Finalize(); - auto tray_state_sender = - builder.AddSystem(*plant_, tray_index); - auto franka_state_sender = - builder.AddSystem(*plant_, franka_index, false); - - // for lcm debugging - auto state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_state_channel, lcm, - drake::systems::TriggerTypeSet( - {drake::systems::TriggerType::kForced}))); - auto tray_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.tray_state_channel, lcm, - drake::systems::TriggerTypeSet( - {drake::systems::TriggerType::kForced}))); - builder.Connect(plant_->get_state_output_port(tray_index), - tray_state_sender->get_input_port_state()); - builder.Connect(plant_->get_state_output_port(franka_index), - franka_state_sender->get_input_port_state()); - builder.Connect(*franka_state_sender, *state_pub); - builder.Connect(*tray_state_sender, *tray_state_pub); - // end lcm debugging - - actuation_port_ = builder.ExportInput(plant_->get_actuation_input_port(), - "franka_sim: robot_efforts"); - tray_state_port_ = builder.ExportOutput(tray_state_sender->get_output_port(), - "franka_sim: tray_state"); - franka_state_port_ = builder.ExportOutput(franka_state_sender->get_output_port(), - "franka_sim: franka_state"); - - builder.BuildInto(this); - this->set_name("FrankaSim"); - -} -} // namespace examples -} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/diagrams/franka_sim_diagram.h b/examples/franka/diagrams/franka_sim_diagram.h deleted file mode 100644 index daf0d8f235..0000000000 --- a/examples/franka/diagrams/franka_sim_diagram.h +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include - -#include "drake/common/drake_copyable.h" -#include "drake/systems/framework/diagram.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/framework/system.h" - -namespace dairlib { -namespace examples { - -class FrankaSimDiagram : public drake::systems::Diagram { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaSimDiagram) - - /// @param[in] urdf filepath containing the osc_running_gains. - FrankaSimDiagram( - std::unique_ptr> plant, - drake::lcm::DrakeLcm* lcm); - - /// @return the input port for the actuation command. - const drake::systems::InputPort& get_input_port_actuation() const { - return this->get_input_port(actuation_port_); - } - - /// @return the output port for the plant state as an OutputVector. - const drake::systems::OutputPort& get_output_port_tray_state() const { - return this->get_output_port(tray_state_port_); - } - - /// @return the output port for the plant state as an OutputVector. - const drake::systems::OutputPort& get_output_port_franka_state() - const { - return this->get_output_port(franka_state_port_); - } - - const drake::multibody::MultibodyPlant& get_plant() { return *plant_; } - - private: - drake::multibody::MultibodyPlant* plant_; - drake::geometry::SceneGraph* scene_graph_; - - drake::systems::InputPortIndex actuation_port_; - drake::systems::OutputPortIndex tray_state_port_; - drake::systems::OutputPortIndex franka_state_port_; -}; - -} // namespace examples -} // namespace dairlib diff --git a/examples/franka/diagrams/full_diagram.cc b/examples/franka/diagrams/full_diagram.cc deleted file mode 100644 index a7857cd503..0000000000 --- a/examples/franka/diagrams/full_diagram.cc +++ /dev/null @@ -1,146 +0,0 @@ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "common/find_resource.h" -#include "examples/franka/diagrams/franka_c3_controller_diagram.h" -#include "examples/franka/diagrams/franka_osc_controller_diagram.h" -#include "examples/franka/diagrams/franka_sim_diagram.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "multibody/multibody_utils.h" -#include "systems/primitives/subvector_pass_through.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -namespace dairlib { - -using drake::geometry::GeometrySet; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::VectorXd; -using examples::controllers::FrankaC3ControllerDiagram; -using examples::controllers::FrankaOSCControllerDiagram; -using systems::RobotInputReceiver; -using systems::RobotOutputSender; -using systems::SubvectorPassThrough; - -int DoMain(int argc, char* argv[]) { - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile( - "examples/franka/parameters/lcm_channels_simulation.yaml"); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - - DiagramBuilder builder; - std::unique_ptr> sim_plant = - std::make_unique>(sim_params.dt); - - auto franka_sim = - builder.AddSystem(std::move(sim_plant), &lcm); - - /// OSC - auto osc_controller = builder.AddSystem( - "examples/franka/parameters/franka_osc_controller_params.yaml", - "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); - - /// C3 plant - auto c3_controller = builder.AddSystem( - "examples/franka/parameters/franka_c3_controller_params.yaml", - "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); - - /* -------------------------------------------------------------------------------------------*/ - auto passthrough = builder.AddSystem( - osc_controller->get_output_port_robot_input().size(), 0, - franka_sim->get_input_port_actuation().size()); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - - //// OSC connections - - // Diagram Connections - builder.Connect(osc_controller->get_output_port_robot_input(), - passthrough->get_input_port()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_position()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_orientation()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_force()); - - builder.Connect(franka_sim->get_output_port_franka_state(), - osc_controller->get_input_port_robot_state()); - builder.Connect(franka_sim->get_output_port_franka_state(), - c3_controller->get_input_port_robot_state()); - builder.Connect(franka_sim->get_output_port_tray_state(), - c3_controller->get_input_ports_object_state()[0]); - - builder.Connect(radio_sub->get_output_port(), - c3_controller->get_input_port_radio()); - builder.Connect(radio_sub->get_output_port(), - osc_controller->get_input_port_radio()); - - builder.Connect(passthrough->get_output_port(), - franka_sim->get_input_port_actuation()); - - auto diagram = builder.Build(); - diagram->set_name("plate_balancing_full_diagram"); - DrawAndSaveDiagramGraph(*diagram); - - drake::systems::Simulator simulator(*diagram); - - simulator.set_publish_every_time_step(true); - simulator.set_publish_at_initialization(true); - simulator.set_target_realtime_rate(sim_params.realtime_rate); - - - auto diagram_context = diagram->CreateDefaultContext(); - auto& plant = franka_sim->get_plant(); - auto& plant_context = - diagram->GetMutableSubsystemContext(plant, diagram_context.get()); - int nq = plant.num_positions(); - int nv = plant.num_velocities(); - VectorXd q = VectorXd::Zero(nq); - q.head(7) = sim_params.q_init_franka; - - q.tail(7) = sim_params.q_init_tray[sim_params.scene_index]; - - plant.SetPositions(&plant_context, q); - - VectorXd v = VectorXd::Zero(nv); - plant.SetVelocities(&plant_context, v); - - simulator.Initialize(); - simulator.AdvanceTo(std::numeric_limits::infinity()); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/forward_kinematics_for_lcs.cc b/examples/franka/forward_kinematics_for_lcs.cc deleted file mode 100644 index c9fdca0831..0000000000 --- a/examples/franka/forward_kinematics_for_lcs.cc +++ /dev/null @@ -1,204 +0,0 @@ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "examples/franka/parameters/franka_c3_controller_params.h" -#include "examples/franka/parameters/franka_c3_scene_params.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "systems/senders/c3_state_sender.h" -#include "examples/franka/systems/c3_trajectory_generator.h" -#include "systems/franka_kinematics.h" -#include "examples/franka/systems/plate_balancing_target.h" -#include "multibody/multibody_utils.h" -#include "solvers/lcs_factory.h" -#include "systems/controllers/c3/lcs_factory_system.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" - -namespace dairlib { - -using dairlib::solvers::LCSFactory; -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; - -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; - -DEFINE_string(controller_settings, - "examples/franka/parameters/franka_c3_controller_params.yaml", - "Controller settings such as channels. Attempting to minimize " - "number of gflags"); -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - // load parameters - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaC3ControllerParams controller_params = - drake::yaml::LoadYamlFile( - FLAGS_controller_settings); - FrankaC3SceneParams scene_params = - drake::yaml::LoadYamlFile( - controller_params.c3_scene_file[controller_params.scene_index]); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - - DiagramBuilder plant_builder; - - MultibodyPlant plant_franka(0.0); - Parser parser_franka(&plant_franka, nullptr); - parser_franka.AddModelsFromUrl(scene_params.franka_model); - drake::multibody::ModelInstanceIndex end_effector_index = - parser_franka.AddModels( - FindResourceOrThrow(scene_params.end_effector_model))[0]; - - RigidTransform X_WI = RigidTransform::Identity(); - plant_franka.WeldFrames(plant_franka.world_frame(), - plant_franka.GetFrameByName("panda_link0"), X_WI); - - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - scene_params.tool_attachment_frame); - plant_franka.WeldFrames( - plant_franka.GetFrameByName("panda_link7"), - plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); - - plant_franka.Finalize(); - auto franka_context = plant_franka.CreateDefaultContext(); - - /// - MultibodyPlant plant_tray(0.0); - Parser parser_tray(&plant_tray, nullptr); - parser_tray.AddModels(scene_params.object_models[0]); - plant_tray.Finalize(); - auto tray_context = plant_tray.CreateDefaultContext(); - - DiagramBuilder builder; - - auto tray_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.tray_state_channel, &lcm)); - auto franka_state_receiver = - builder.AddSystem(plant_franka); - auto tray_state_receiver = - builder.AddSystem(plant_tray); - auto reduced_order_model_receiver = - builder.AddSystem( - plant_franka, franka_context.get(), plant_tray, tray_context.get(), - scene_params.end_effector_name, "tray", - controller_params.include_end_effector_orientation); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - auto radio_to_vector = builder.AddSystem(); - - std::vector state_names = { - "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", - "tray_qx", "tray_qy", "tray_qz", "tray_x", - "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", - "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", - "tray_vz", "tray_vz", "tray_vz", - }; - auto c3_state_sender = - builder.AddSystem(3 + 7 + 3 + 6, state_names); - auto plate_balancing_target = - builder.AddSystem( - plant_tray, scene_params.end_effector_thickness, - controller_params.near_target_threshold); - plate_balancing_target->SetRemoteControlParameters( - controller_params.first_target[controller_params.scene_index], controller_params.second_target[controller_params.scene_index], - controller_params.third_target[controller_params.scene_index], controller_params.x_scale, - controller_params.y_scale, controller_params.z_scale); - std::vector input_sizes = {3, 7, 3, 6}; - auto target_state_mux = - builder.AddSystem(input_sizes); - auto end_effector_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(3)); - auto tray_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(6)); - builder.Connect(end_effector_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(2)); - builder.Connect(tray_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(3)); - - - auto c3_actual_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_actual_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_target_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_target_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - builder.Connect(*radio_sub, *radio_to_vector); - builder.Connect(tray_state_receiver->get_output_port(), - plate_balancing_target->get_input_port_tray_state()); - builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), - target_state_mux->get_input_port(0)); - builder.Connect(plate_balancing_target->get_output_port_tray_target(), - target_state_mux->get_input_port(1)); - builder.Connect(tray_state_sub->get_output_port(), - tray_state_receiver->get_input_port()); - builder.Connect(radio_to_vector->get_output_port(), - plate_balancing_target->get_input_port_radio()); - builder.Connect(franka_state_receiver->get_output_port(), - reduced_order_model_receiver->get_input_port_franka_state()); - builder.Connect(tray_state_receiver->get_output_port(), - *(reduced_order_model_receiver->get_input_ports_object_state().at(0))); - builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), - c3_state_sender->get_input_port_actual_state()); - builder.Connect(target_state_mux->get_output_port(), - c3_state_sender->get_input_port_target_state()); - builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), - c3_actual_state_publisher->get_input_port()); - builder.Connect(c3_state_sender->get_output_port_target_c3_state(), - c3_target_state_publisher->get_input_port()); - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_forward_kinematics")); - - // Run lcm-driven simulation - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), franka_state_receiver, - lcm_channel_params.franka_state_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - - LcmHandleSubscriptionsUntil( - &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); - loop.Simulate(); - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_bridge_driver_in.cc b/examples/franka/franka_bridge_driver_in.cc deleted file mode 100644 index 44552f3f10..0000000000 --- a/examples/franka/franka_bridge_driver_in.cc +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/parameters/franka_drake_lcm_driver_channels.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "systems/franka_state_translator.h" -#include "multibody/multibody_utils.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" - -using drake::math::RigidTransform; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::Simulator; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; - -using dairlib::systems::RobotInputReceiver; -using dairlib::systems::RobotOutputSender; -using dairlib::systems::SubvectorPassThrough; -using dairlib::systems::TimestampedVector; - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_hardware.yaml", - "Filepath containing lcm channels"); -DEFINE_string(franka_driver_channels, - "common/parameters/franka_drake_lcm_driver_channels.yaml", - "Filepath containing drake franka driver channels"); - -namespace dairlib { - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaDrakeLcmDriverChannels franka_driver_channel_params = - drake::yaml::LoadYamlFile(FLAGS_franka_driver_channels); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - - DiagramBuilder builder; - - MultibodyPlant plant(0.0); - - Parser parser(&plant); - parser.AddModelsFromUrl(sim_params.franka_model); - Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); - RigidTransform R_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - R_X_W); - plant.Finalize(); - - auto pos_map = multibody::MakeNameToPositionsMap(plant); - auto vel_map = multibody::MakeNameToVelocitiesMap(plant); - auto act_map = multibody::MakeNameToActuatorsMap(plant); - - auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); - auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); - auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); - - /* -------------------------------------------------------------------------------------------*/ - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - auto franka_command_pub = - builder.AddSystem(LcmPublisherSystem::Make( - franka_driver_channel_params.franka_command_channel, &lcm, - 1.0 / 1000.0)); - auto franka_command_translator = builder.AddSystem(); - - builder.Connect(*franka_command_translator, *franka_command_pub); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_bridge_driver_in")); - - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), franka_command_translator, - lcm_channel_params.franka_input_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - loop.Simulate(); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_bridge_driver_out.cc b/examples/franka/franka_bridge_driver_out.cc deleted file mode 100644 index b88b0be2ab..0000000000 --- a/examples/franka/franka_bridge_driver_out.cc +++ /dev/null @@ -1,104 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/parameters/franka_drake_lcm_driver_channels.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "systems/franka_state_translator.h" -#include "multibody/multibody_utils.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" - -using drake::math::RigidTransform; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::Simulator; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; - -using dairlib::systems::RobotInputReceiver; -using dairlib::systems::RobotOutputSender; -using dairlib::systems::SubvectorPassThrough; -using dairlib::systems::TimestampedVector; - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_hardware.yaml", - "Filepath containing lcm channels"); -DEFINE_string(franka_driver_channels, - "common/parameters/franka_drake_lcm_driver_channels.yaml", - "Filepath containing drake franka driver channels"); - -namespace dairlib { - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaDrakeLcmDriverChannels franka_driver_channel_params = - drake::yaml::LoadYamlFile(FLAGS_franka_driver_channels); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - - DiagramBuilder builder; - - MultibodyPlant plant(0.0); - - Parser parser(&plant); - parser.AddModelsFromUrl(sim_params.franka_model); - Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); - RigidTransform R_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - R_X_W); - plant.Finalize(); - - auto pos_map = multibody::MakeNameToPositionsMap(plant); - auto vel_map = multibody::MakeNameToVelocitiesMap(plant); - auto act_map = multibody::MakeNameToActuatorsMap(plant); - - auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); - auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); - auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); - - /* -------------------------------------------------------------------------------------------*/ - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - - auto franka_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_state_channel, &lcm, - 1.0 / 1000.0)); - auto franka_state_translator = builder.AddSystem( - pos_names, vel_names, act_names); - - builder.Connect(*franka_state_translator, *franka_state_pub); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_bridge_driver_out")); - - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), franka_state_translator, - franka_driver_channel_params.franka_status_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - loop.Simulate(); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_c3_controller.cc b/examples/franka/franka_c3_controller.cc deleted file mode 100644 index 6fcb7ac999..0000000000 --- a/examples/franka/franka_c3_controller.cc +++ /dev/null @@ -1,336 +0,0 @@ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "examples/franka/parameters/franka_c3_controller_params.h" -#include "examples/franka/parameters/franka_c3_scene_params.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "systems/senders/c3_state_sender.h" -#include "examples/franka/systems/c3_trajectory_generator.h" -#include "systems/franka_kinematics.h" -#include "examples/franka/systems/plate_balancing_target.h" -#include "multibody/multibody_utils.h" -#include "solvers/lcs_factory.h" -#include "systems/controllers/c3/lcs_factory_system.h" -#include "systems/controllers/c3/c3_controller.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/c3_output_systems.h" - -namespace dairlib { - -using dairlib::solvers::LCSFactory; -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; - -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; - -DEFINE_string(controller_settings, - "examples/franka/parameters/franka_c3_controller_params.yaml", - "Controller settings such as channels. Attempting to minimize " - "number of gflags"); -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - // load parameters - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaC3ControllerParams controller_params = - drake::yaml::LoadYamlFile( - FLAGS_controller_settings); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - C3Options c3_options = drake::yaml::LoadYamlFile( - controller_params.c3_options_file[controller_params.scene_index]); - FrankaC3SceneParams scene_params = - drake::yaml::LoadYamlFile( - controller_params.c3_scene_file[controller_params.scene_index]); - drake::solvers::SolverOptions solver_options = - drake::yaml::LoadYamlFile( - FindResourceOrThrow(controller_params.osqp_settings_file)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - - - MultibodyPlant plant_franka(0.0); - Parser parser_franka(&plant_franka, nullptr); - parser_franka.AddModelsFromUrl(scene_params.franka_model); - drake::multibody::ModelInstanceIndex end_effector_index = - parser_franka.AddModels( - FindResourceOrThrow(scene_params.end_effector_model))[0]; - - RigidTransform X_WI = RigidTransform::Identity(); - plant_franka.WeldFrames(plant_franka.world_frame(), - plant_franka.GetFrameByName("panda_link0"), X_WI); - - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - scene_params.tool_attachment_frame); - plant_franka.WeldFrames( - plant_franka.GetFrameByName("panda_link7"), - plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); - - plant_franka.Finalize(); - auto franka_context = plant_franka.CreateDefaultContext(); - - /// - MultibodyPlant plant_tray(0.0); - Parser parser_tray(&plant_tray, nullptr); - parser_tray.AddModels(scene_params.object_models[0]); - plant_tray.Finalize(); - auto tray_context = plant_tray.CreateDefaultContext(); - - /// - DiagramBuilder plant_builder; - auto [plant_for_lcs, scene_graph] = - AddMultibodyPlantSceneGraph(&plant_builder, 0.0); - Parser lcs_parser(&plant_for_lcs); - lcs_parser.SetAutoRenaming(true); - lcs_parser.AddModels(scene_params.end_effector_lcs_model); - - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = lcs_parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant_for_lcs.WeldFrames( - plant_for_lcs.world_frame(), - plant_for_lcs.GetFrameByName("base", environment_model_indices[i]), - T_E_W); - } - for (int i = 0; i < scene_params.object_models.size(); ++i) { - lcs_parser.AddModels(scene_params.object_models[i]); - } - - plant_for_lcs.WeldFrames(plant_for_lcs.world_frame(), - plant_for_lcs.GetFrameByName("base_link"), X_WI); - plant_for_lcs.Finalize(); - std::unique_ptr> plant_for_lcs_autodiff = - drake::systems::System::ToAutoDiffXd(plant_for_lcs); - - auto plant_diagram = plant_builder.Build(); - std::unique_ptr> diagram_context = - plant_diagram->CreateDefaultContext(); - auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext( - plant_for_lcs, diagram_context.get()); - auto plate_context_ad = plant_for_lcs_autodiff->CreateDefaultContext(); - - /// - std::vector end_effector_contact_points = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("plate")); - for (int i = 0; i < environment_model_indices.size(); ++i) { - std::vector - environment_support_contact_points = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("base", - environment_model_indices[i])); - end_effector_contact_points.insert( - end_effector_contact_points.end(), - environment_support_contact_points.begin(), - environment_support_contact_points.end()); - } - std::vector tray_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("tray")); - std::unordered_map> - contact_geoms; - contact_geoms["PLATE"] = end_effector_contact_points; - contact_geoms["TRAY"] = tray_geoms; - - std::vector> contact_pairs; - for (auto geom_id : contact_geoms["PLATE"]) { - contact_pairs.emplace_back(geom_id, contact_geoms["TRAY"][0]); - } - - DiagramBuilder builder; - - auto tray_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.tray_state_channel, &lcm)); - auto franka_state_receiver = - builder.AddSystem(plant_franka); - auto tray_state_receiver = - builder.AddSystem(plant_tray); - auto reduced_order_model_receiver = - builder.AddSystem( - plant_franka, franka_context.get(), plant_tray, tray_context.get(), - scene_params.end_effector_name, "tray", - controller_params.include_end_effector_orientation); - auto actor_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_actor_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - auto object_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_object_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - auto c3_output_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_debug_output_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_target_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_target_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_actual_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_actual_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_forces_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_force_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - auto radio_to_vector = builder.AddSystem(); - - auto plate_balancing_target = - builder.AddSystem( - plant_tray, scene_params.end_effector_thickness, - controller_params.near_target_threshold); - plate_balancing_target->SetRemoteControlParameters( - controller_params.first_target[controller_params.scene_index], - controller_params.second_target[controller_params.scene_index], - controller_params.third_target[controller_params.scene_index], - controller_params.x_scale, controller_params.y_scale, - controller_params.z_scale); - std::vector input_sizes = {3, 7, 3, 6}; - auto target_state_mux = - builder.AddSystem(input_sizes); - auto end_effector_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(3)); - builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), - target_state_mux->get_input_port(0)); - builder.Connect(plate_balancing_target->get_output_port_tray_target(), - target_state_mux->get_input_port(1)); - builder.Connect(end_effector_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(2)); - builder.Connect(plate_balancing_target->get_output_port_tray_velocity_target(), - target_state_mux->get_input_port(3)); - - - auto lcs_factory = builder.AddSystem( - plant_for_lcs, plant_for_lcs_context, *plant_for_lcs_autodiff, - *plate_context_ad, contact_pairs, c3_options); - auto controller = - builder.AddSystem(plant_for_lcs, c3_options); - auto c3_trajectory_generator = - builder.AddSystem(plant_for_lcs, - c3_options); - std::vector state_names = { - "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", - "tray_qx", "tray_qy", "tray_qz", "tray_x", - "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", - "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", - "tray_vz", "tray_vz", "tray_vz", - }; - auto c3_state_sender = - builder.AddSystem(3 + 7 + 3 + 6, state_names); - c3_trajectory_generator->SetPublishEndEffectorOrientation( - controller_params.include_end_effector_orientation); - auto c3_output_sender = builder.AddSystem(); - controller->SetOsqpSolverOptions(solver_options); - - builder.Connect(*radio_sub, *radio_to_vector); - builder.Connect(franka_state_receiver->get_output_port(), - reduced_order_model_receiver->get_input_port_franka_state()); - builder.Connect(target_state_mux->get_output_port(), - controller->get_input_port_target()); - builder.Connect(lcs_factory->get_output_port_lcs(), - controller->get_input_port_lcs()); - builder.Connect(tray_state_sub->get_output_port(), - tray_state_receiver->get_input_port()); - builder.Connect(tray_state_receiver->get_output_port(), - *(reduced_order_model_receiver->get_input_ports_object_state().at(0))); - builder.Connect(tray_state_receiver->get_output_port(), - plate_balancing_target->get_input_port_tray_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - controller->get_input_port_lcs_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - lcs_factory->get_input_port_lcs_state()); - builder.Connect(radio_to_vector->get_output_port(), - plate_balancing_target->get_input_port_radio()); - builder.Connect(controller->get_output_port_c3_solution(), - c3_trajectory_generator->get_input_port_c3_solution()); - builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), - c3_output_sender->get_input_port_lcs_contact_info()); - builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), - actor_trajectory_sender->get_input_port()); - builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), - object_trajectory_sender->get_input_port()); - builder.Connect(target_state_mux->get_output_port(), - c3_state_sender->get_input_port_target_state()); - builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), - c3_state_sender->get_input_port_actual_state()); - builder.Connect(c3_state_sender->get_output_port_target_c3_state(), - c3_target_state_publisher->get_input_port()); - builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), - c3_actual_state_publisher->get_input_port()); - builder.Connect(controller->get_output_port_c3_solution(), - c3_output_sender->get_input_port_c3_solution()); - builder.Connect(controller->get_output_port_c3_intermediates(), - c3_output_sender->get_input_port_c3_intermediates()); - builder.Connect(c3_output_sender->get_output_port_c3_debug(), - c3_output_publisher->get_input_port()); - builder.Connect(c3_output_sender->get_output_port_c3_force(), - c3_forces_publisher->get_input_port()); - // builder.Connect(c3_output_sender->get_output_port_next_c3_input(), - // lcs_factory->get_input_port_lcs_input()); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_c3_controller")); - plant_diagram->set_name(("franka_c3_plant")); - // DrawAndSaveDiagramGraph(*plant_diagram); - - // Run lcm-driven simulation - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), franka_state_receiver, - lcm_channel_params.franka_state_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - // auto& controller_context = loop.get_diagram()->GetMutableSubsystemContext( - // *controller, &loop.get_diagram_mutable_context()); - // controller->get_input_port_target().FixValue(&controller_context, x_des); - LcmHandleSubscriptionsUntil( - &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); - loop.Simulate(); - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_c3_controller_two_objects.cc b/examples/franka/franka_c3_controller_two_objects.cc deleted file mode 100644 index e558fe547e..0000000000 --- a/examples/franka/franka_c3_controller_two_objects.cc +++ /dev/null @@ -1,348 +0,0 @@ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "examples/franka/parameters/franka_c3_controller_params.h" -#include "examples/franka/parameters/franka_c3_scene_params.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "systems/senders/c3_state_sender.h" -#include "examples/franka/systems/c3_trajectory_generator.h" -#include "systems/franka_kinematics.h" -#include "examples/franka/systems/plate_balancing_target.h" -#include "multibody/multibody_utils.h" -#include "solvers/lcs_factory.h" -#include "systems/controllers/c3/lcs_factory_system.h" -#include "systems/controllers/c3/c3_controller.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/c3_output_systems.h" - -namespace dairlib { - -using dairlib::solvers::LCSFactory; -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; - -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; - -DEFINE_string(controller_settings, - "examples/franka/parameters/franka_c3_controller_params.yaml", - "Controller settings such as channels. Attempting to minimize " - "number of gflags"); -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - // load parameters - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaC3ControllerParams controller_params = - drake::yaml::LoadYamlFile( - FLAGS_controller_settings); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - C3Options c3_options = drake::yaml::LoadYamlFile( - controller_params.c3_options_file[controller_params.scene_index]); - FrankaC3SceneParams scene_params = - drake::yaml::LoadYamlFile( - controller_params.c3_scene_file[controller_params.scene_index]); - drake::solvers::SolverOptions solver_options = - drake::yaml::LoadYamlFile( - FindResourceOrThrow(controller_params.osqp_settings_file)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - - DiagramBuilder plant_builder; - - MultibodyPlant plant_franka(0.0); - Parser parser_franka(&plant_franka, nullptr); - parser_franka.AddModelsFromUrl(scene_params.franka_model); - drake::multibody::ModelInstanceIndex end_effector_index = - parser_franka.AddModels( - FindResourceOrThrow(scene_params.end_effector_model))[0]; - - RigidTransform X_WI = RigidTransform::Identity(); - plant_franka.WeldFrames(plant_franka.world_frame(), - plant_franka.GetFrameByName("panda_link0"), X_WI); - - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - scene_params.tool_attachment_frame); - plant_franka.WeldFrames( - plant_franka.GetFrameByName("panda_link7"), - plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); - - plant_franka.Finalize(); - auto franka_context = plant_franka.CreateDefaultContext(); - - /// - MultibodyPlant plant_tray(0.0); - Parser parser_tray(&plant_tray, nullptr); - - auto tray_index = parser_tray.AddModels(scene_params.object_models[0])[0]; - auto object_index = parser_tray.AddModels(scene_params.object_models[1])[0]; - plant_tray.Finalize(); - auto tray_context = plant_tray.CreateDefaultContext(); - - /// - auto [plant_for_lcs, scene_graph] = - AddMultibodyPlantSceneGraph(&plant_builder, 0.0); - Parser lcs_parser(&plant_for_lcs); - lcs_parser.SetAutoRenaming(true); - lcs_parser.AddModels(scene_params.end_effector_lcs_model); - - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = lcs_parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant_for_lcs.WeldFrames( - plant_for_lcs.world_frame(), - plant_for_lcs.GetFrameByName("base", environment_model_indices[i]), - T_E_W); - } - for (int i = 0; i < scene_params.object_models.size(); ++i) { - lcs_parser.AddModels(scene_params.object_models[i]); - } - - plant_for_lcs.WeldFrames(plant_for_lcs.world_frame(), - plant_for_lcs.GetFrameByName("base_link"), X_WI); - plant_for_lcs.Finalize(); - std::unique_ptr> plant_for_lcs_autodiff = - drake::systems::System::ToAutoDiffXd(plant_for_lcs); - - auto plant_diagram = plant_builder.Build(); - std::unique_ptr> diagram_context = - plant_diagram->CreateDefaultContext(); - auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext( - plant_for_lcs, diagram_context.get()); - auto plate_context_ad = plant_for_lcs_autodiff->CreateDefaultContext(); - - /// - std::vector end_effector_contact_points = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("plate")); - for (int i = 0; i < environment_model_indices.size(); ++i) { - std::vector - environment_support_contact_points = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("base", - environment_model_indices[i])); - end_effector_contact_points.insert( - end_effector_contact_points.end(), - environment_support_contact_points.begin(), - environment_support_contact_points.end()); - } - std::vector tray_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("tray")); - std::vector object_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("base")); - - std::vector> contact_pairs; - for (auto geom_id : end_effector_contact_points) { - contact_pairs.emplace_back(geom_id, tray_geoms[0]); - } - for (auto geom_id : object_geoms) { - contact_pairs.emplace_back(tray_geoms[0], geom_id); - } - - DiagramBuilder builder; - - auto tray_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.tray_state_channel, &lcm)); - auto object_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.object_state_channel, &lcm)); - auto franka_state_receiver = - builder.AddSystem(plant_franka); - auto tray_state_receiver = - builder.AddSystem(plant_tray, tray_index); - auto object_state_receiver = - builder.AddSystem(plant_tray, object_index); - auto reduced_order_model_receiver = - builder.AddSystem( - plant_franka, franka_context.get(), plant_tray, tray_context.get(), - scene_params.end_effector_name, "tray", - controller_params.include_end_effector_orientation); - auto actor_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_actor_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - auto object_trajectory_sender = builder.AddSystem( - LcmPublisherSystem::Make( - lcm_channel_params.c3_object_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - auto c3_output_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_debug_output_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_target_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_target_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_actual_state_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_actual_state_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_forces_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_force_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - auto radio_to_vector = builder.AddSystem(); - - auto plate_balancing_target = - builder.AddSystem( - plant_tray, scene_params.end_effector_thickness, - controller_params.near_target_threshold); - plate_balancing_target->SetRemoteControlParameters( - controller_params.first_target[controller_params.scene_index], - controller_params.second_target[controller_params.scene_index], - controller_params.third_target[controller_params.scene_index], - controller_params.x_scale, controller_params.y_scale, - controller_params.z_scale); - std::vector input_sizes = {3, 7, 3, 6}; - auto target_state_mux = - builder.AddSystem(input_sizes); - auto end_effector_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(3)); - auto tray_zero_velocity_source = - builder.AddSystem( - VectorXd::Zero(6)); - builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), - target_state_mux->get_input_port(0)); - builder.Connect(plate_balancing_target->get_output_port_tray_target(), - target_state_mux->get_input_port(1)); - builder.Connect(end_effector_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(2)); - builder.Connect(tray_zero_velocity_source->get_output_port(), - target_state_mux->get_input_port(3)); - auto lcs_factory = builder.AddSystem( - plant_for_lcs, plant_for_lcs_context, *plant_for_lcs_autodiff, - *plate_context_ad, contact_pairs, c3_options); - auto controller = - builder.AddSystem(plant_for_lcs, c3_options); - auto c3_trajectory_generator = - builder.AddSystem(plant_for_lcs, - c3_options); - std::vector state_names = { - "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", - "tray_qx", "tray_qy", "tray_qz", "tray_x", - "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", - "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", - "tray_vz", "tray_vz", "tray_vz", - }; - auto c3_state_sender = - builder.AddSystem(3 + 7 + 3 + 6, state_names); - c3_trajectory_generator->SetPublishEndEffectorOrientation( - controller_params.include_end_effector_orientation); - auto c3_output_sender = builder.AddSystem(); - controller->SetOsqpSolverOptions(solver_options); - - builder.Connect(*radio_sub, *radio_to_vector); - builder.Connect(franka_state_receiver->get_output_port(), - reduced_order_model_receiver->get_input_port_franka_state()); - builder.Connect(target_state_mux->get_output_port(), - controller->get_input_port_target()); - builder.Connect(lcs_factory->get_output_port_lcs(), - controller->get_input_port_lcs()); - builder.Connect(tray_state_sub->get_output_port(), - tray_state_receiver->get_input_port()); - builder.Connect(tray_state_receiver->get_output_port(), - *(reduced_order_model_receiver->get_input_ports_object_state()[0])); - builder.Connect(tray_state_receiver->get_output_port(), - plate_balancing_target->get_input_port_tray_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - controller->get_input_port_lcs_state()); - builder.Connect(reduced_order_model_receiver->get_output_port(), - lcs_factory->get_input_port_lcs_state()); - builder.Connect(radio_to_vector->get_output_port(), - plate_balancing_target->get_input_port_radio()); - builder.Connect(controller->get_output_port_c3_solution(), - c3_trajectory_generator->get_input_port_c3_solution()); - builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), - c3_output_sender->get_input_port_lcs_contact_info()); - builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), - actor_trajectory_sender->get_input_port()); - builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), - object_trajectory_sender->get_input_port()); - builder.Connect(target_state_mux->get_output_port(), - c3_state_sender->get_input_port_target_state()); - builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), - c3_state_sender->get_input_port_actual_state()); - builder.Connect(c3_state_sender->get_output_port_target_c3_state(), - c3_target_state_publisher->get_input_port()); - builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), - c3_actual_state_publisher->get_input_port()); - builder.Connect(controller->get_output_port_c3_solution(), - c3_output_sender->get_input_port_c3_solution()); - builder.Connect(controller->get_output_port_c3_intermediates(), - c3_output_sender->get_input_port_c3_intermediates()); - builder.Connect(c3_output_sender->get_output_port_c3_debug(), - c3_output_publisher->get_input_port()); - builder.Connect(c3_output_sender->get_output_port_c3_force(), - c3_forces_publisher->get_input_port()); - // builder.Connect(c3_output_sender->get_output_port_next_c3_input(), - // lcs_factory->get_input_port_lcs_input()); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_c3_controller")); - plant_diagram->set_name(("franka_c3_plant")); - // DrawAndSaveDiagramGraph(*plant_diagram); - - // Run lcm-driven simulation - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), franka_state_receiver, - lcm_channel_params.franka_state_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - // auto& controller_context = loop.get_diagram()->GetMutableSubsystemContext( - // *controller, &loop.get_diagram_mutable_context()); - // controller->get_input_port_target().FixValue(&controller_context, x_des); - LcmHandleSubscriptionsUntil( - &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); - loop.Simulate(); - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_hardware.pmd b/examples/franka/franka_hardware.pmd deleted file mode 100644 index b20047cf2c..0000000000 --- a/examples/franka/franka_hardware.pmd +++ /dev/null @@ -1,81 +0,0 @@ -group "operator" { - cmd "visualizer" { - exec = "bazel-bin/examples/franka/franka_visualizer --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; - host = "localhost"; - } - cmd "xbox" { - exec = "bazel-bin/examples/Cassie/cassie_xbox_remote"; - host = "localhost"; - } - cmd "logger" { - exec = "python3 start_logging.py hw"; - host = "localhost"; - } - cmd "record_video" { - exec = "python3 record_video.py"; - host = "localhost"; - } -} - -group "controllers (hardware)" { - cmd "franka_osc" { - exec = "bazel-bin/examples/franka/franka_osc_controller --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; - host = "localhost"; - } - cmd "franka_c3" { - exec = "bazel-bin/examples/franka/franka_c3_controller --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; - host = "localhost"; - } -} - -group "debug" { - cmd "lcm-spy" { - exec = "bazel-bin/lcmtypes/dair-lcm-spy"; - host = "localhost"; - } -} - -group "drivers" { - cmd "franka_driver_out" { - exec = "bazel-bin/examples/franka/franka_bridge_driver_out"; - host = "localhost"; - } - cmd "franka_driver_in" { - exec = "bazel-bin/examples/franka/franka_bridge_driver_in"; - host = "localhost"; - } - cmd "position_driver" { - exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=position"; - host = "franka_control"; - } - cmd "torque_driver" { - exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; - host = "franka_control"; - } -} - -script "start_operator_commands" { - restart cmd "visualizer"; - restart cmd "xbox"; -} - -script "start_experiment" { - stop cmd "franka_driver_out"; - stop cmd "franka_driver_in"; - stop cmd "franka_osc"; - stop cmd "torque_driver"; - start cmd "record_video"; - start cmd "logger"; - wait ms 1000; - start cmd "franka_driver_out"; - start cmd "franka_driver_in"; - start cmd "torque_driver"; - start cmd "franka_osc"; -} - -script "stop_experiment" { - stop group "drivers"; - stop group "controllers (hardware)"; - stop cmd "record_video"; - stop cmd "logger"; -} diff --git a/examples/franka/franka_lcm_ros_bridge.cc b/examples/franka/franka_lcm_ros_bridge.cc deleted file mode 100644 index 7c0252e06c..0000000000 --- a/examples/franka/franka_lcm_ros_bridge.cc +++ /dev/null @@ -1,125 +0,0 @@ -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "franka_msgs/FrankaState.h" -#include "ros/ros.h" -#include "sensor_msgs/JointState.h" -#include "std_msgs/Float64MultiArray.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/robot_lcm_systems.h" -#include "systems/ros/franka_ros_lcm_conversions.h" -#include "systems/ros/parameters/franka_ros_channels.h" -#include "systems/ros/ros_publisher_system.h" -#include "systems/ros/ros_subscriber_system.h" -#include "systems/system_utils.h" - -using drake::math::RigidTransform; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::Simulator; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; - -using dairlib::systems::LcmToRosTimestampedVector; -using dairlib::systems::RobotInputReceiver; -using dairlib::systems::RobotOutputSender; -using dairlib::systems::RosPublisherSystem; -using dairlib::systems::RosSubscriberSystem; -using dairlib::systems::RosToLcmObjectState; -using dairlib::systems::RosToLcmRobotState; -using dairlib::systems::SubvectorPassThrough; -using dairlib::systems::TimestampedVector; - -// Shutdown ROS gracefully and then exit -void SigintHandler(int sig) { - ros::shutdown(); - exit(sig); -} - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_hardware.yaml", - "Filepath containing lcm channels"); -DEFINE_string(ros_channels, "systems/ros/parameters/franka_ros_channels.yaml", - "Filepath containing ROS channels"); - -namespace dairlib { - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaRosChannels ros_channel_params = - drake::yaml::LoadYamlFile(FLAGS_ros_channels); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - - ros::init(argc, argv, "run_lcm_to_ros"); - ros::NodeHandle node_handle; - - drake::lcm::DrakeLcm drake_lcm("udpm://239.255.76.67:7667?ttl=0"); - DiagramBuilder builder; - - MultibodyPlant plant(0.0); - - Parser parser(&plant); - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); - RigidTransform R_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - R_X_W); - plant.Finalize(); - - /* -------------------------------------------------------------------------------------------*/ - auto robot_input_receiver = builder.AddSystem(plant); - auto lcm_to_ros_robot_input = builder.AddSystem(7); - auto robot_input_ros_publisher = builder.AddSystem( - systems::RosPublisherSystem::Make( - ros_channel_params.franka_input_channel, &node_handle, - {drake::systems::TriggerType::kForced})); - - builder.Connect(robot_input_receiver->get_output_port(), - lcm_to_ros_robot_input->get_input_port()); - builder.Connect(lcm_to_ros_robot_input->get_output_port(), - robot_input_ros_publisher->get_input_port()); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_lcm_ros_bridge")); - const auto& diagram = *owned_diagram; - - // figure out what the arguments to this mean - ros::AsyncSpinner spinner(1); - spinner.start(); - signal(SIGINT, SigintHandler); - - systems::LcmDrivenLoop loop( - &drake_lcm, std::move(owned_diagram), robot_input_receiver, - lcm_channel_params.franka_input_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - loop.Simulate(); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_osc_controller.cc b/examples/franka/franka_osc_controller.cc deleted file mode 100644 index 1cba2b9e70..0000000000 --- a/examples/franka/franka_osc_controller.cc +++ /dev/null @@ -1,287 +0,0 @@ - -#include -#include -#include - -#include "common/eigen_utils.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_osc_controller_params.h" -#include "systems/controllers/osc/end_effector_force.h" -#include "systems/controllers/osc/end_effector_orientation.h" -#include "systems/controllers/osc/end_effector_position.h" -#include "lcm/lcm_trajectory.h" -#include "multibody/multibody_utils.h" -#include "systems/controllers/gravity_compensator.h" -#include "systems/controllers/osc/external_force_tracking_data.h" -#include "systems/controllers/osc/joint_space_tracking_data.h" -#include "systems/controllers/osc/operational_space_control.h" -#include "systems/controllers/osc/relative_translation_tracking_data.h" -#include "systems/controllers/osc/rot_space_tracking_data.h" -#include "systems/controllers/osc/trans_space_tracking_data.h" -#include "systems/framework/lcm_driven_loop.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/lcm_trajectory_systems.h" - -#include "drake/common/find_resource.h" -#include "drake/common/yaml/yaml_io.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/lcm/lcm_publisher_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" - -namespace dairlib { - -using drake::math::RigidTransform; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::TriggerType; -using drake::systems::TriggerTypeSet; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::MatrixXd; -using Eigen::Vector3d; -using Eigen::VectorXd; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; - -using systems::controllers::ExternalForceTrackingData; -using systems::controllers::JointSpaceTrackingData; -using systems::controllers::RelativeTranslationTrackingData; -using systems::controllers::RotTaskSpaceTrackingData; -using systems::controllers::TransTaskSpaceTrackingData; - -DEFINE_string(osqp_settings, - "examples/franka/parameters/franka_osc_qp_settings.yaml", - "Filepath containing qp settings"); -DEFINE_string(controller_parameters, - "examples/franka/parameters/franka_osc_controller_params.yaml", - "Controller settings such as channels. Attempting to minimize " - "number of gflags"); -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - - // load parameters - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaControllerParams controller_params = - drake::yaml::LoadYamlFile( - FLAGS_controller_parameters); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - OSCGains gains = drake::yaml::LoadYamlFile( - FindResourceOrThrow(FLAGS_controller_parameters), {}, {}, yaml_options); - drake::solvers::SolverOptions solver_options = - drake::yaml::LoadYamlFile( - FindResourceOrThrow(FLAGS_osqp_settings)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - DiagramBuilder builder; - - drake::multibody::MultibodyPlant plant(0.0); - Parser parser(&plant, nullptr); - parser.AddModelsFromUrl(controller_params.franka_model); - - RigidTransform X_WI = RigidTransform::Identity(); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - X_WI); - - if (!controller_params.end_effector_name.empty()) { - drake::multibody::ModelInstanceIndex end_effector_index = parser.AddModels( - FindResourceOrThrow(controller_params.end_effector_model))[0]; - RigidTransform T_EE_W = - RigidTransform(drake::math::RotationMatrix(), - controller_params.tool_attachment_frame); - plant.WeldFrames(plant.GetFrameByName("panda_link7"), - plant.GetFrameByName(controller_params.end_effector_name, - end_effector_index), - T_EE_W); - } else { - std::cout << "OSC plant has been constructed with no end effector." - << std::endl; - } - - plant.Finalize(); - auto plant_context = plant.CreateDefaultContext(); - - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - auto state_receiver = builder.AddSystem(plant); - auto end_effector_trajectory_sub = builder.AddSystem( - LcmSubscriberSystem::Make( - lcm_channel_params.c3_actor_channel, &lcm)); - auto end_effector_position_receiver = - builder.AddSystem( - "end_effector_position_target"); - auto end_effector_force_receiver = - builder.AddSystem( - "end_effector_force_target"); - auto end_effector_orientation_receiver = - builder.AddSystem( - "end_effector_orientation_target"); - auto franka_command_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_input_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto osc_command_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.osc_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto franka_command_sender = - builder.AddSystem(plant); - auto osc_command_sender = - builder.AddSystem(plant); - auto end_effector_trajectory = - builder.AddSystem(plant, - plant_context.get(), controller_params.neutral_position, false, - controller_params.end_effector_name); - end_effector_trajectory->SetRemoteControlParameters( - controller_params.neutral_position, controller_params.x_scale, - controller_params.y_scale, controller_params.z_scale); - auto end_effector_orientation_trajectory = - builder.AddSystem(); - end_effector_orientation_trajectory->SetTrackOrientation( - controller_params.track_end_effector_orientation); - auto end_effector_force_trajectory = - builder.AddSystem(); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - auto osc = builder.AddSystem( - plant, plant, plant_context.get(), plant_context.get(), false); - if (controller_params.publish_debug_info) { - auto osc_debug_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.osc_debug_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - builder.Connect(osc->get_output_port_osc_debug(), - osc_debug_pub->get_input_port()); - } - - auto end_effector_position_tracking_data = - std::make_unique( - "end_effector_target", controller_params.K_p_end_effector, - controller_params.K_d_end_effector, controller_params.W_end_effector, - plant, plant); - end_effector_position_tracking_data->AddPointToTrack( - controller_params.end_effector_name); - const VectorXd& end_effector_acceleration_limits = - controller_params.end_effector_acceleration * Vector3d::Ones(); - end_effector_position_tracking_data->SetCmdAccelerationBounds( - -end_effector_acceleration_limits, end_effector_acceleration_limits); - auto mid_link_position_tracking_data_for_rel = - std::make_unique( - "panda_joint2_target", controller_params.K_p_mid_link, - controller_params.K_d_mid_link, controller_params.W_mid_link, plant, - plant); - mid_link_position_tracking_data_for_rel->AddJointToTrack("panda_joint2", - "panda_joint2dot"); - - auto end_effector_force_tracking_data = - std::make_unique( - "end_effector_force", controller_params.W_ee_lambda, plant, plant, - controller_params.end_effector_name, Vector3d::Zero()); - - auto end_effector_orientation_tracking_data = - std::make_unique( - "end_effector_orientation_target", - controller_params.K_p_end_effector_rot, - controller_params.K_d_end_effector_rot, - controller_params.W_end_effector_rot, plant, plant); - end_effector_orientation_tracking_data->AddFrameToTrack( - controller_params.end_effector_name); - Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4); - orientation_target(0) = 1; - osc->AddTrackingData(std::move(end_effector_position_tracking_data)); - osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel), - 1.6 * VectorXd::Ones(1)); - osc->AddTrackingData(std::move(end_effector_orientation_tracking_data)); - osc->AddForceTrackingData(std::move(end_effector_force_tracking_data)); - osc->SetAccelerationCostWeights(gains.W_acceleration); - osc->SetInputCostWeights(gains.W_input_regularization); - osc->SetInputSmoothingCostWeights(gains.W_input_smoothing_regularization); - osc->SetAccelerationConstraints( - controller_params.enforce_acceleration_constraints); - - osc->SetContactFriction(controller_params.mu); - osc->SetOsqpSolverOptions(solver_options); - - osc->Build(); - - if (controller_params.cancel_gravity_compensation) { - auto gravity_compensator = - builder.AddSystem(plant, - *plant_context); - builder.Connect(osc->get_output_port_osc_command(), - gravity_compensator->get_input_port()); - builder.Connect(gravity_compensator->get_output_port(), - franka_command_sender->get_input_port()); - } else { - if (FLAGS_lcm_channels == - "examples/franka/parameters/lcm_channels_hardware.yaml") { - std::cerr << "Using hardware lcm channels but not cancelling gravity " - "compensation. Please check the OSC settings" - << std::endl; - return -1; - } - builder.Connect(osc->get_output_port_osc_command(), - franka_command_sender->get_input_port(0)); - } - - builder.Connect(radio_sub->get_output_port(), - end_effector_trajectory->get_input_port_radio()); - builder.Connect(radio_sub->get_output_port(), - end_effector_orientation_trajectory->get_input_port_radio()); - builder.Connect(radio_sub->get_output_port(), - end_effector_force_trajectory->get_input_port_radio()); - builder.Connect(franka_command_sender->get_output_port(), - franka_command_pub->get_input_port()); - builder.Connect(osc_command_sender->get_output_port(), - osc_command_pub->get_input_port()); - builder.Connect(osc->get_output_port_osc_command(), - osc_command_sender->get_input_port(0)); - - builder.Connect(state_receiver->get_output_port(0), - osc->get_input_port_robot_output()); - builder.Connect(end_effector_trajectory_sub->get_output_port(), - end_effector_position_receiver->get_input_port_trajectory()); - builder.Connect(end_effector_trajectory_sub->get_output_port(), - end_effector_force_receiver->get_input_port_trajectory()); - builder.Connect( - end_effector_trajectory_sub->get_output_port(), - end_effector_orientation_receiver->get_input_port_trajectory()); - builder.Connect(end_effector_position_receiver->get_output_port(0), - end_effector_trajectory->get_input_port_trajectory()); - builder.Connect( - end_effector_orientation_receiver->get_output_port(0), - end_effector_orientation_trajectory->get_input_port_trajectory()); - builder.Connect(end_effector_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_target")); - builder.Connect( - end_effector_orientation_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_orientation_target")); - builder.Connect(end_effector_force_receiver->get_output_port(0), - end_effector_force_trajectory->get_input_port_trajectory()); - builder.Connect(end_effector_force_trajectory->get_output_port(0), - osc->get_input_port_tracking_data("end_effector_force")); - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_osc_controller")); - // Run lcm-driven simulation - systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), state_receiver, - lcm_channel_params.franka_state_channel, true); - DrawAndSaveDiagramGraph(*loop.get_diagram()); - loop.Simulate(); - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_ros_lcm_bridge.cc b/examples/franka/franka_ros_lcm_bridge.cc deleted file mode 100644 index f61ae024ba..0000000000 --- a/examples/franka/franka_ros_lcm_bridge.cc +++ /dev/null @@ -1,191 +0,0 @@ -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/find_resource.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "franka_msgs/FrankaState.h" -#include "ros/ros.h" -#include "sensor_msgs/JointState.h" -#include "nav_msgs/Odometry.h" -#include "systems/robot_lcm_systems.h" -#include "systems/ros/franka_ros_lcm_conversions.h" -#include "systems/ros/parameters/franka_ros_channels.h" -#include "systems/ros/ros_publisher_system.h" -#include "systems/ros/ros_subscriber_system.h" -#include "systems/system_utils.h" - -using drake::math::RigidTransform; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::Simulator; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; - -using dairlib::systems::LcmToRosTimestampedVector; -using dairlib::systems::RobotInputReceiver; -using dairlib::systems::RobotOutputSender; -using dairlib::systems::RosPublisherSystem; -using dairlib::systems::RosSubscriberSystem; -using dairlib::systems::RosToLcmObjectState; -using dairlib::systems::RosToLcmRobotState; -using dairlib::systems::SubvectorPassThrough; -using dairlib::systems::TimestampedVector; -// using dairlib::systems::ROSToRobotOutputLCM; - -// Shutdown ROS gracefully and then exit -void SigintHandler(int sig) { - ros::shutdown(); - exit(sig); -} - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_hardware.yaml", - "Filepath containing lcm channels"); -DEFINE_string(ros_channels, "systems/ros/parameters/franka_ros_channels.yaml", - "Filepath containing ROS channels"); - -namespace dairlib { - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaRosChannels ros_channel_params = - drake::yaml::LoadYamlFile(FLAGS_ros_channels); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - - ros::init(argc, argv, "run_ros_to_lcm"); - ros::NodeHandle node_handle; - - drake::lcm::DrakeLcm drake_lcm("udpm://239.255.76.67:7667?ttl=0"); - DiagramBuilder builder; - - MultibodyPlant plant(0.0); - - Parser parser(&plant); - auto franka_index = - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - auto tray_index = - parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; - Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); - RigidTransform R_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - R_X_W); - plant.Finalize(); - - /// convert franka joint states to lcm - auto franka_joint_state_ros_subscriber = - builder.AddSystem(RosSubscriberSystem::Make( - ros_channel_params.franka_state_channel, &node_handle)); - auto ros_to_lcm_robot_state = builder.AddSystem(RosToLcmRobotState::Make( - plant.num_positions(franka_index), plant.num_velocities(franka_index), - plant.num_actuators(franka_index))); - - // change this to output correctly (i.e. when ros subscriber gets new message) - auto robot_output_lcm_publisher = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_state_channel, &drake_lcm, - {drake::systems::TriggerType::kForced})); - /// connections - builder.Connect(franka_joint_state_ros_subscriber->get_output_port(), - ros_to_lcm_robot_state->get_input_port()); - builder.Connect(ros_to_lcm_robot_state->get_output_port(), - robot_output_lcm_publisher->get_input_port()); - - /* -------------------------------------------------------------------------------------------*/ - auto tray_object_state_ros_subscriber = - builder.AddSystem(RosSubscriberSystem::Make( - ros_channel_params.tray_state_channel, &node_handle)); - auto ros_to_lcm_object_state = builder.AddSystem( - RosToLcmObjectState::Make(plant, tray_index, "tray")); - - // change this to output correctly (i.e. when ros subscriber gets new message) - auto tray_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.tray_state_channel, &drake_lcm, - {drake::systems::TriggerType::kForced})); - /// connections - builder.Connect(tray_object_state_ros_subscriber->get_output_port(), - ros_to_lcm_object_state->get_input_port()); - builder.Connect(ros_to_lcm_object_state->get_output_port(), - tray_state_pub->get_input_port()); - - /* -------------------------------------------------------------------------------------------*/ - - auto owned_diagram = builder.Build(); - owned_diagram->set_name(("franka_ros_lcm_bridge")); - const auto& diagram = *owned_diagram; - DrawAndSaveDiagramGraph(diagram); - drake::systems::Simulator simulator(std::move(owned_diagram)); - auto& diagram_context = simulator.get_mutable_context(); - - // figure out what the arguments to this mean - ros::AsyncSpinner spinner(1); - spinner.start(); - signal(SIGINT, SigintHandler); - - // Wait for the first message. - drake::log()->info("Waiting for first ROS message from Franka"); - int old_message_count = 0; - old_message_count = - franka_joint_state_ros_subscriber->WaitForMessage(old_message_count); - - // Initialize the context based on the first message. - const double t0 = franka_joint_state_ros_subscriber->message_time(); - diagram_context.SetTime(t0); - simulator.Initialize(); - - drake::log()->info("franka ros-lcm bridge started"); - - while (true) { - old_message_count = - franka_joint_state_ros_subscriber->WaitForMessage(old_message_count); - const double time = franka_joint_state_ros_subscriber->message_time(); - - // Check if we are very far ahead or behind - // (likely due to a restart of the driving clock) - if (time > simulator.get_context().get_time() + 1.0 || - time < simulator.get_context().get_time()) { - std::cout << "Dispatcher time is " << simulator.get_context().get_time() - << ", but stepping to " << time << std::endl; - std::cout << "Difference is too large, resetting dispatcher time." - << std::endl; - simulator.get_mutable_context().SetTime(time); - simulator.Initialize(); - } - - simulator.AdvanceTo(time); - - // Force-publish via the diagram - diagram.CalcForcedUnrestrictedUpdate(diagram_context, - &diagram_context.get_mutable_state()); - diagram.CalcForcedDiscreteVariableUpdate( - diagram_context, &diagram_context.get_mutable_discrete_state()); - diagram.ForcedPublish(diagram_context); - } - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/franka_sim.cc b/examples/franka/franka_sim.cc deleted file mode 100644 index f047338d8d..0000000000 --- a/examples/franka/franka_sim.cc +++ /dev/null @@ -1,231 +0,0 @@ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "common/find_resource.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "examples/franka/parameters/franka_sim_scene_params.h" -#include "examples/franka/systems/external_force_generator.h" -#include "multibody/multibody_utils.h" -#include "systems/primitives/radio_parser.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" - -namespace dairlib { - -using dairlib::systems::SubvectorPassThrough; -using drake::geometry::GeometrySet; -using drake::geometry::SceneGraph; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::systems::Context; -using drake::systems::DiagramBuilder; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using drake::trajectories::PiecewisePolynomial; -using multibody::MakeNameToPositionsMap; -using multibody::MakeNameToVelocitiesMap; -using systems::AddActuationRecieverAndStateSenderLcm; - -using Eigen::MatrixXd; -using Eigen::Vector3d; -using Eigen::VectorXd; - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int DoMain(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - // load parameters - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaSimSceneParams scene_params = - drake::yaml::LoadYamlFile( - sim_params.sim_scene_file[sim_params.scene_index]); - - // load urdf and sphere - DiagramBuilder builder; - double sim_dt = sim_params.dt; - auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, sim_dt); - - Parser parser(&plant); - parser.SetAutoRenaming(true); - drake::multibody::ModelInstanceIndex franka_index = - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - drake::multibody::ModelInstanceIndex end_effector_index = - parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; - drake::multibody::ModelInstanceIndex tray_index = - parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; - drake::multibody::ModelInstanceIndex object_index = - parser.AddModels(FindResourceOrThrow(sim_params.object_model))[0]; - multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); - - RigidTransform X_WI = RigidTransform::Identity(); - Vector3d franka_origin = Eigen::VectorXd::Zero(3); - - RigidTransform T_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - RigidTransform T_EE_W = RigidTransform( - drake::math::RotationMatrix(), sim_params.tool_attachment_frame); - - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - T_X_W); - plant.WeldFrames(plant.GetFrameByName("panda_link7"), - plant.GetFrameByName("plate", end_effector_index), T_EE_W); - - // we WANT to model collisions between link5 and the supports - const drake::geometry::GeometrySet& franka_geom_set = - plant.CollectRegisteredGeometries({&plant.GetBodyByName("panda_link0"), - &plant.GetBodyByName("panda_link1"), - &plant.GetBodyByName("panda_link2"), - &plant.GetBodyByName("panda_link3"), - &plant.GetBodyByName("panda_link4")}); - - drake::geometry::GeometrySet support_geom_set; - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant.WeldFrames(plant.world_frame(), - plant.GetFrameByName("base", environment_model_indices[i]), - T_E_W); - support_geom_set.Add(plant.GetCollisionGeometriesForBody( - plant.GetBodyByName("base", environment_model_indices[i]))); - } - plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"supports", support_geom_set}, {"franka", franka_geom_set}); - - const drake::geometry::GeometrySet& franka_only_geom_set = - plant.CollectRegisteredGeometries({ - &plant.GetBodyByName("panda_link2"), - &plant.GetBodyByName("panda_link3"), - &plant.GetBodyByName("panda_link4"), - &plant.GetBodyByName("panda_link5"), - &plant.GetBodyByName("panda_link6"), - &plant.GetBodyByName("panda_link7"), - &plant.GetBodyByName("panda_link8"), - }); - auto tray_collision_set = GeometrySet( - plant.GetCollisionGeometriesForBody(plant.GetBodyByName("tray"))); - plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"franka", franka_only_geom_set}, {"tray", tray_collision_set}); - - plant.Finalize(); - /* -------------------------------------------------------------------------------------------*/ - - drake::lcm::DrakeLcm drake_lcm; - auto lcm = - builder.AddSystem(&drake_lcm); - AddActuationRecieverAndStateSenderLcm( - &builder, plant, lcm, lcm_channel_params.franka_input_channel, - lcm_channel_params.franka_state_channel, sim_params.franka_publish_rate, - franka_index, sim_params.publish_efforts, sim_params.actuator_delay); - auto tray_state_sender = - builder.AddSystem(plant, sim_params.publish_object_velocities, tray_index); - auto tray_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.tray_state_channel, lcm, - 1.0 / sim_params.tray_publish_rate)); - auto object_state_sender = - builder.AddSystem(plant, sim_params.publish_object_velocities, object_index); - auto object_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.object_state_channel, lcm, - 1.0 / sim_params.object_publish_rate)); - - builder.Connect(plant.get_state_output_port(tray_index), - tray_state_sender->get_input_port_state()); - builder.Connect(tray_state_sender->get_output_port(), - tray_state_pub->get_input_port()); - builder.Connect(plant.get_state_output_port(object_index), - object_state_sender->get_input_port_state()); - builder.Connect(object_state_sender->get_output_port(), - object_state_pub->get_input_port()); - - auto external_force_generator = builder.AddSystem( - plant.GetBodyByName("tray").index()); - external_force_generator->SetRemoteControlParameters( - sim_params.external_force_scaling[0], - sim_params.external_force_scaling[1], - sim_params.external_force_scaling[2]); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &drake_lcm)); - auto radio_to_vector = builder.AddSystem(); - builder.Connect(*radio_sub, *radio_to_vector); - builder.Connect(radio_to_vector->get_output_port(), - external_force_generator->get_input_port_radio()); - builder.Connect(external_force_generator->get_output_port_spatial_force(), - plant.get_applied_spatial_force_input_port()); - - int nq = plant.num_positions(); - int nv = plant.num_velocities(); - - if (sim_params.visualize_drake_sim) { - drake::visualization::AddDefaultVisualization(&builder); - } - - auto diagram = builder.Build(); - - drake::systems::Simulator simulator(*diagram); - - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); - simulator.set_target_realtime_rate(sim_params.realtime_rate); - - auto& plant_context = diagram->GetMutableSubsystemContext( - plant, &simulator.get_mutable_context()); - - VectorXd q = VectorXd::Zero(nq); - - q.head(plant.num_positions(franka_index)) = sim_params.q_init_franka; - - q.segment(plant.num_positions(franka_index), - plant.num_positions(tray_index)) = - sim_params.q_init_tray[sim_params.scene_index]; - q.tail(plant.num_positions(object_index)) = - sim_params.q_init_object[sim_params.scene_index]; - - plant.SetPositions(&plant_context, q); - - VectorXd v = VectorXd::Zero(nv); - plant.SetVelocities(&plant_context, v); - - simulator.Initialize(); - simulator.AdvanceTo(std::numeric_limits::infinity()); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/franka_sim.pmd b/examples/franka/franka_sim.pmd deleted file mode 100644 index f12f036d8d..0000000000 --- a/examples/franka/franka_sim.pmd +++ /dev/null @@ -1,69 +0,0 @@ -group "simulations" { - cmd "franka_sim" { - exec = "bazel-bin/examples/franka/franka_sim"; - host = "localhost"; - } - cmd "franka_kinematics" { - exec = "bazel-bin/examples/franka/forward_kinematics_for_lcs"; - host = "localhost"; - } -} - -group "operator" { - cmd "visualizer" { - exec = "bazel-bin/examples/franka/franka_visualizer"; - host = "localhost"; - } - cmd "xbox" { - exec = "bazel-bin/examples/Cassie/cassie_xbox_remote"; - host = "localhost"; - } - cmd "start_logging" { - exec = "python3 start_logging.py sim"; - host = "localhost"; - } -} - -group "controllers" { - cmd "franka_osc" { - exec = "bazel-bin/examples/franka/franka_osc_controller"; - host = "localhost"; - } - cmd "franka_c3" { - exec = "bazel-bin/examples/franka/franka_c3_controller"; - host = "localhost"; - } - cmd "mujoco_mpc" { - exec = "../mujoco_mpc/build/bin/standalone_controller"; - host = "localhost"; - } -} - -group "debug" { - cmd "lcm-spy" { - exec = "bazel-bin/lcmtypes/dair-lcm-spy"; - host = "localhost"; - } -} - -script "c3_mpc" { - start cmd "franka_sim"; - start cmd "franka_osc"; - start cmd "franka_c3"; -} - -script "mjmpc_with_drake_sim" { - start cmd "franka_sim"; - start cmd "franka_osc"; - start cmd "mujoco_mpc"; - start cmd "franka_kinematics"; -} - -script "start_operator_commands" { - start cmd "visualizer"; -} - -script "stop_controllers_and_simulators" { - stop group "simulations"; - stop group "controllers"; -} diff --git a/examples/franka/franka_visualizer.cc b/examples/franka/franka_visualizer.cc deleted file mode 100644 index d370bd6d71..0000000000 --- a/examples/franka/franka_visualizer.cc +++ /dev/null @@ -1,326 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "common/find_resource.h" -#include "dairlib/lcmt_robot_output.hpp" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "examples/franka/parameters/franka_sim_scene_params.h" -#include "multibody/com_pose_system.h" -#include "multibody/multibody_utils.h" -#include "multibody/visualization_utils.h" -#include "systems/primitives/subvector_pass_through.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -#include "systems/trajectory_optimization/lcm_trajectory_systems.h" -#include "systems/visualization/lcm_visualization_systems.h" - -#include "drake/common/find_resource.h" -#include "drake/common/yaml/yaml_io.h" -#include "drake/geometry/drake_visualizer.h" -#include "drake/geometry/meshcat_visualizer.h" -#include "drake/geometry/meshcat_visualizer_params.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" -#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" - -namespace dairlib { - -using Eigen::MatrixXd; -using Eigen::Vector3d; -using Eigen::VectorXd; - -using dairlib::systems::ObjectStateReceiver; -using dairlib::systems::RobotOutputReceiver; -using dairlib::systems::SubvectorPassThrough; -using drake::geometry::DrakeVisualizer; -using drake::geometry::SceneGraph; -using drake::geometry::Sphere; -using drake::math::RigidTransformd; -using drake::multibody::MultibodyPlant; -using drake::multibody::RigidBody; -using drake::multibody::SpatialInertia; -using drake::multibody::UnitInertia; -using drake::systems::Simulator; -using drake::systems::lcm::LcmSubscriberSystem; -using drake::systems::rendering::MultibodyPositionToGeometryPose; - -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; - -DEFINE_string(lcm_channels, - "examples/franka/parameters/lcm_channels_simulation.yaml", - "Filepath containing lcm channels"); - -int do_main(int argc, char* argv[]) { - gflags::ParseCommandLineFlags(&argc, &argv, true); - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile(FLAGS_lcm_channels); - FrankaSimSceneParams scene_params = - drake::yaml::LoadYamlFile( - sim_params.sim_scene_file[sim_params.scene_index]); - - drake::systems::DiagramBuilder builder; - - SceneGraph& scene_graph = *builder.AddSystem(); - scene_graph.set_name("scene_graph"); - - MultibodyPlant plant(0.0); - - Parser parser(&plant, &scene_graph); - parser.SetAutoRenaming(true); - drake::multibody::ModelInstanceIndex franka_index = - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - drake::multibody::ModelInstanceIndex end_effector_index = - parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; - drake::multibody::ModelInstanceIndex tray_index = - parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; - drake::multibody::ModelInstanceIndex object_index = - parser.AddModels(FindResourceOrThrow(sim_params.object_model))[0]; - multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); - - RigidTransform X_WI = RigidTransform::Identity(); - Vector3d franka_origin = Eigen::VectorXd::Zero(3); - - RigidTransform R_X_W = RigidTransform( - drake::math::RotationMatrix(), franka_origin); - RigidTransform T_EE_W = RigidTransform( - drake::math::RotationMatrix(), sim_params.tool_attachment_frame); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - R_X_W); - plant.WeldFrames(plant.GetFrameByName("panda_link7"), - plant.GetFrameByName("plate", end_effector_index), T_EE_W); - - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant.WeldFrames(plant.world_frame(), - plant.GetFrameByName("base", environment_model_indices[i]), - T_E_W); - } - - plant.Finalize(); - - auto lcm = builder.AddSystem(); - - // Create state receiver. - auto franka_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.franka_state_channel, lcm)); - auto tray_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.tray_state_channel, lcm)); - auto object_state_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.object_state_channel, lcm)); - auto franka_state_receiver = - builder.AddSystem(plant, franka_index); - auto tray_state_receiver = - builder.AddSystem(plant, tray_index); - auto object_state_receiver = - builder.AddSystem(plant, object_index); - - auto franka_passthrough = builder.AddSystem( - franka_state_receiver->get_output_port(0).size(), 0, - plant.num_positions(franka_index)); - auto robot_time_passthrough = builder.AddSystem( - franka_state_receiver->get_output_port(0).size(), - franka_state_receiver->get_output_port(0).size() - 1, 1); - auto tray_passthrough = builder.AddSystem( - tray_state_receiver->get_output_port(0).size(), 0, - plant.num_positions(tray_index)); - auto object_passthrough = builder.AddSystem( - tray_state_receiver->get_output_port(0).size(), 0, - plant.num_positions(object_index)); - - std::vector input_sizes = {plant.num_positions(franka_index), - plant.num_positions(tray_index), - plant.num_positions(object_index)}; - auto mux = - builder.AddSystem>(input_sizes); - - auto trajectory_sub_actor = builder.AddSystem( - LcmSubscriberSystem::Make( - lcm_channel_params.c3_actor_channel, lcm)); - auto trajectory_sub_tray = builder.AddSystem( - LcmSubscriberSystem::Make( - lcm_channel_params.c3_object_channel, lcm)); - auto trajectory_sub_force = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.c3_force_channel, lcm)); - - auto c3_state_actual_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.c3_actual_state_channel, lcm)); - auto c3_state_target_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.c3_target_state_channel, lcm)); - auto to_pose = - builder.AddSystem>(plant); - - drake::geometry::MeshcatVisualizerParams params; - params.publish_period = 1.0 / sim_params.visualizer_publish_rate; - auto meshcat = std::make_shared(); - - meshcat->SetCameraPose(scene_params.camera_pose, - scene_params.camera_target); - - if (sim_params.visualize_workspace) { - double width = sim_params.world_x_limits[sim_params.scene_index][1] - - sim_params.world_x_limits[sim_params.scene_index][0]; - double depth = sim_params.world_y_limits[sim_params.scene_index][1] - - sim_params.world_y_limits[sim_params.scene_index][0]; - double height = sim_params.world_z_limits[sim_params.scene_index][1] - - sim_params.world_z_limits[sim_params.scene_index][0]; - Vector3d workspace_center = { - 0.5 * (sim_params.world_x_limits[sim_params.scene_index][1] + - sim_params.world_x_limits[sim_params.scene_index][0]), - 0.5 * (sim_params.world_y_limits[sim_params.scene_index][1] + - sim_params.world_y_limits[sim_params.scene_index][0]), - 0.5 * (sim_params.world_z_limits[sim_params.scene_index][1] + - sim_params.world_z_limits[sim_params.scene_index][0])}; - meshcat->SetObject("c3_state/workspace", - drake::geometry::Box(width, depth, height), - {1, 0, 0, 0.2}); - meshcat->SetTransform("c3_state/workspace", - RigidTransformd(workspace_center)); - } - if (sim_params.visualize_center_of_mass_plan) { - auto trajectory_drawer_actor = - builder.AddSystem( - meshcat, "end_effector_position_target"); - auto trajectory_drawer_object = - builder.AddSystem( - meshcat, "object_position_target"); - trajectory_drawer_actor->SetLineColor(drake::geometry::Rgba({1, 0, 0, 1})); - trajectory_drawer_object->SetLineColor(drake::geometry::Rgba({0, 0, 1, 1})); - trajectory_drawer_actor->SetNumSamples(40); - trajectory_drawer_object->SetNumSamples(40); - builder.Connect(trajectory_sub_actor->get_output_port(), - trajectory_drawer_actor->get_input_port_trajectory()); - builder.Connect(trajectory_sub_tray->get_output_port(), - trajectory_drawer_object->get_input_port_trajectory()); - } - - if (sim_params.visualize_pose_trace) { - auto object_pose_drawer = builder.AddSystem( - meshcat, - FindResourceOrThrow("examples/franka/urdf/tray.sdf"), - "object_position_target", "object_orientation_target"); - auto end_effector_pose_drawer = builder.AddSystem( - meshcat, FindResourceOrThrow(sim_params.end_effector_model), - "end_effector_position_target", "end_effector_orientation_target"); - - builder.Connect(trajectory_sub_tray->get_output_port(), - object_pose_drawer->get_input_port_trajectory()); - builder.Connect(trajectory_sub_actor->get_output_port(), - end_effector_pose_drawer->get_input_port_trajectory()); - } - - if (sim_params.visualize_c3_object_state || sim_params.visualize_c3_end_effector_state) { - auto c3_target_drawer = - builder.AddSystem(meshcat, sim_params.visualize_c3_object_state, sim_params.visualize_c3_end_effector_state); - builder.Connect(c3_state_actual_sub->get_output_port(), - c3_target_drawer->get_input_port_c3_state_actual()); - builder.Connect(c3_state_target_sub->get_output_port(), - c3_target_drawer->get_input_port_c3_state_target()); - } - - if (sim_params.visualize_c3_forces) { - auto end_effector_force_drawer = builder.AddSystem( - meshcat, "end_effector_position_target", "end_effector_force_target", - "lcs_force_trajectory"); - builder.Connect( - trajectory_sub_actor->get_output_port(), - end_effector_force_drawer->get_input_port_actor_trajectory()); - builder.Connect( - trajectory_sub_force->get_output_port(), - end_effector_force_drawer->get_input_port_force_trajectory()); - builder.Connect(robot_time_passthrough->get_output_port(), - end_effector_force_drawer->get_input_port_robot_time()); - } - - builder.Connect(franka_passthrough->get_output_port(), - mux->get_input_port(0)); - builder.Connect(tray_passthrough->get_output_port(), mux->get_input_port(1)); - builder.Connect(object_passthrough->get_output_port(), - mux->get_input_port(2)); - builder.Connect(*mux, *to_pose); - builder.Connect( - to_pose->get_output_port(), - scene_graph.get_source_pose_port(plant.get_source_id().value())); - builder.Connect(*franka_state_receiver, *franka_passthrough); - builder.Connect(*franka_state_receiver, *robot_time_passthrough); - builder.Connect(*tray_state_receiver, *tray_passthrough); - builder.Connect(*object_state_receiver, *object_passthrough); - builder.Connect(*franka_state_sub, *franka_state_receiver); - builder.Connect(*tray_state_sub, *tray_state_receiver); - builder.Connect(*object_state_sub, *object_state_receiver); - - auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( - &builder, scene_graph, meshcat, std::move(params)); - - auto diagram = builder.Build(); - auto context = diagram->CreateDefaultContext(); - - auto& franka_state_sub_context = - diagram->GetMutableSubsystemContext(*franka_state_sub, context.get()); - auto& tray_state_sub_context = - diagram->GetMutableSubsystemContext(*tray_state_sub, context.get()); - auto& object_state_sub_context = - diagram->GetMutableSubsystemContext(*object_state_sub, context.get()); - franka_state_receiver->InitializeSubscriberPositions( - plant, franka_state_sub_context); - tray_state_receiver->InitializeSubscriberPositions(plant, - tray_state_sub_context); - object_state_receiver->InitializeSubscriberPositions( - plant, object_state_sub_context); - - /// Use the simulator to drive at a fixed rate - /// If set_publish_every_time_step is true, this publishes twice - /// Set realtime rate. Otherwise, runs as fast as possible - auto simulator = - std::make_unique>(*diagram, std::move(context)); - simulator->set_publish_every_time_step(false); - simulator->set_publish_at_initialization(false); - simulator->set_target_realtime_rate( - 1.0); // may need to change this to param.real_time_rate? - simulator->Initialize(); - - simulator->AdvanceTo(std::numeric_limits::infinity()); - // meshcat->get_mutable_recording().set_loop_mode(drake::geometry::MeshcatAnimation::LoopMode::kLoopRepeat); - // meshcat->StartRecording(); - - // simulator->AdvanceTo(18.0); - // meshcat->StopRecording(); - // meshcat->PublishRecording(); - // std::ofstream outfile("visualization.html"); - // outfile << meshcat->StaticHtml() < -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/eigen_utils.h" -#include "common/find_resource.h" -#include "examples/franka/diagrams/franka_c3_controller_diagram.h" -#include "examples/franka/diagrams/franka_osc_controller_diagram.h" -#include "examples/franka/parameters/franka_c3_controller_params.h" -#include "examples/franka/parameters/franka_lcm_channels.h" -#include "examples/franka/parameters/franka_sim_params.h" -#include "examples/franka/parameters/franka_sim_scene_params.h" -#include "multibody/multibody_utils.h" -#include "systems/primitives/radio_parser.h" -#include "systems/primitives/subvector_pass_through.h" -#include "systems/robot_lcm_systems.h" -#include "systems/system_utils.h" -namespace dairlib { - -using drake::geometry::GeometrySet; -using drake::math::RigidTransform; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::Parser; -using drake::systems::DiagramBuilder; -using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; -using Eigen::VectorXd; -using examples::controllers::FrankaC3ControllerDiagram; -using examples::controllers::FrankaOSCControllerDiagram; -using systems::RobotInputReceiver; -using systems::RobotOutputSender; -using systems::SubvectorPassThrough; - -int DoMain(int argc, char* argv[]) { - drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); - - drake::yaml::LoadYamlOptions yaml_options; - yaml_options.allow_yaml_with_no_cpp = true; - FrankaLcmChannels lcm_channel_params = - drake::yaml::LoadYamlFile( - "examples/franka/parameters/lcm_channels_simulation.yaml"); - // load parameters - FrankaSimParams sim_params = drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_sim_params.yaml"); - FrankaSimSceneParams scene_params = - drake::yaml::LoadYamlFile( - sim_params.sim_scene_file[sim_params.scene_index]); - FrankaC3ControllerParams c3_params = - drake::yaml::LoadYamlFile( - "examples/franka/parameters/franka_c3_controller_params.yaml"); - C3Options c3_options = drake::yaml::LoadYamlFile( - c3_params.c3_options_file[c3_params.scene_index]); - - DiagramBuilder builder; - - /// Sim Start - double sim_dt = sim_params.dt; - auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, sim_dt); - - Parser parser(&plant); - parser.SetAutoRenaming(true); - drake::multibody::ModelInstanceIndex franka_index = - parser.AddModelsFromUrl(sim_params.franka_model)[0]; - drake::multibody::ModelInstanceIndex c3_end_effector_index = - parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; - drake::multibody::ModelInstanceIndex tray_index = - parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; - multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); - - RigidTransform T_X_W = RigidTransform( - drake::math::RotationMatrix(), Eigen::VectorXd::Zero(3)); - RigidTransform T_EE_W = RigidTransform( - drake::math::RotationMatrix(), sim_params.tool_attachment_frame); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), - T_X_W); - plant.WeldFrames(plant.GetFrameByName("panda_link7"), - plant.GetFrameByName("plate", c3_end_effector_index), - T_EE_W); - - const drake::geometry::GeometrySet& franka_geom_set = - plant.CollectRegisteredGeometries({&plant.GetBodyByName("panda_link0"), - &plant.GetBodyByName("panda_link1"), - &plant.GetBodyByName("panda_link2"), - &plant.GetBodyByName("panda_link3"), - &plant.GetBodyByName("panda_link4")}); - - drake::geometry::GeometrySet support_geom_set; - std::vector environment_model_indices; - environment_model_indices.resize(scene_params.environment_models.size()); - for (int i = 0; i < scene_params.environment_models.size(); ++i) { - environment_model_indices[i] = parser.AddModels( - FindResourceOrThrow(scene_params.environment_models[i]))[0]; - RigidTransform T_E_W = - RigidTransform(drake::math::RollPitchYaw( - scene_params.environment_orientations[i]), - scene_params.environment_positions[i]); - plant.WeldFrames( - plant.world_frame(), - plant.GetFrameByName("base", environment_model_indices[i]), - T_E_W); - support_geom_set.Add(plant.GetCollisionGeometriesForBody(plant.GetBodyByName("base", - environment_model_indices[i]))); - } - plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"supports", support_geom_set}, {"franka", franka_geom_set}); - - - const drake::geometry::GeometrySet& paddle_geom_set = - plant.CollectRegisteredGeometries({ - &plant.GetBodyByName("panda_link2"), - &plant.GetBodyByName("panda_link3"), - &plant.GetBodyByName("panda_link4"), - &plant.GetBodyByName("panda_link5"), - &plant.GetBodyByName("panda_link6"), - &plant.GetBodyByName("panda_link8"), - }); - auto tray_collision_set = GeometrySet( - plant.GetCollisionGeometriesForBody(plant.GetBodyByName("tray"))); - plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {"paddle", paddle_geom_set}, {"tray", tray_collision_set}); - - plant.Finalize(); - - /// OSC - auto osc_controller = builder.AddSystem( - "examples/franka/parameters/franka_osc_controller_params.yaml", - "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); - - /// C3 plant - auto c3_controller = builder.AddSystem( - "examples/franka/parameters/franka_c3_controller_params.yaml", c3_options, - "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); - - /* -------------------------------------------------------------------------------------------*/ - auto passthrough = builder.AddSystem( - osc_controller->get_output_port_robot_input().size(), 0, - plant.get_actuation_input_port().size()); - auto tray_state_sender = - builder.AddSystem(plant, tray_index); - auto franka_state_sender = - builder.AddSystem(plant, franka_index, false); - auto state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.franka_state_channel, &lcm, - drake::systems::TriggerTypeSet( - {drake::systems::TriggerType::kForced}))); - auto tray_state_pub = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.tray_state_channel, &lcm, - drake::systems::TriggerTypeSet( - {drake::systems::TriggerType::kForced}))); - auto radio_sub = - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.radio_channel, &lcm)); - - //// OSC connections - - auto radio_to_vector = builder.AddSystem(); - builder.Connect(radio_sub->get_output_port(), - radio_to_vector->get_input_port()); - // Diagram Connections - builder.Connect(osc_controller->get_output_port_robot_input(), - passthrough->get_input_port()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_position()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_orientation()); - builder.Connect(c3_controller->get_output_port_mpc_plan(), - osc_controller->get_input_port_end_effector_force()); - - builder.Connect(franka_state_sender->get_output_port(), - osc_controller->get_input_port_robot_state()); - builder.Connect(franka_state_sender->get_output_port(), - c3_controller->get_input_port_robot_state()); - builder.Connect(tray_state_sender->get_output_port(), - c3_controller->get_input_port_object_state()); - builder.Connect(radio_to_vector->get_output_port(), - c3_controller->get_input_port_radio()); - builder.Connect(radio_to_vector->get_output_port(), - osc_controller->get_input_port_radio()); - - builder.Connect(*franka_state_sender, *state_pub); - builder.Connect(tray_state_sender->get_output_port(), - tray_state_pub->get_input_port()); - builder.Connect(plant.get_state_output_port(franka_index), - franka_state_sender->get_input_port_state()); - builder.Connect(plant.get_state_output_port(tray_index), - tray_state_sender->get_input_port_state()); - builder.Connect(passthrough->get_output_port(), - plant.get_actuation_input_port()); - - int nq = plant.num_positions(); - int nv = plant.num_velocities(); - - if (sim_params.visualize_drake_sim) { - drake::visualization::AddDefaultVisualization(&builder); - } - - auto diagram = builder.Build(); - diagram->set_name("plate_balancing_full_diagram"); - DrawAndSaveDiagramGraph(*diagram); - - drake::systems::Simulator simulator(*diagram); - - simulator.set_publish_every_time_step(true); - simulator.set_publish_at_initialization(true); - simulator.set_target_realtime_rate(sim_params.realtime_rate); - - auto& plant_context = diagram->GetMutableSubsystemContext( - plant, &simulator.get_mutable_context()); - - VectorXd q = VectorXd::Zero(nq); - q.head(plant.num_positions(franka_index)) = sim_params.q_init_franka; - - q.tail(plant.num_positions(tray_index)) = - sim_params.q_init_tray[sim_params.scene_index]; - - plant.SetPositions(&plant_context, q); - - VectorXd v = VectorXd::Zero(nv); - plant.SetVelocities(&plant_context, v); - - simulator.Initialize(); - simulator.AdvanceTo(std::numeric_limits::infinity()); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml b/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml deleted file mode 100644 index cf813acb07..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml +++ /dev/null @@ -1,54 +0,0 @@ -admm_iter: 4 -rho: 0.1 -rho_scale: 1 -num_threads: 6 -delta_option: 1 -# options are 'MIQP' or 'QP' -projection_type: 'MIQP' -# options are 'stewart_and_trinkle' or 'anitescu -contact_model: 'stewart_and_trinkle' - -mu: 0.4 -mu_plate: 0.4 -dt: 0.05 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 3 -N: 5 - -# matrix scaling -w_Q: 20 -w_R: 0 -# Penalty on all decision variables, assuming scalar -w_G: 1 -# Penalty on all decision variables, assuming scalar -w_U: 0.5 - -# n_lambda = 2 * n_contacts + 2 * n_contacts * num_friction_directions -# size = n_x ( 7 + 3 + 6 + 3 ) + n_lambda (2 * 3 + 2 * 3 * 2) + n_u (3) = 40 for stewart and trinkle -# size = n_x ( 7 + 3 + 6 + 3 ) + n_lambda (2 * 3 * 2) + n_u (3) = 34 for anitescu -g_size: 49 -u_size: 49 -#g_size: 34 -#u_size: 34 -# State Tracking Error, assuming diagonal -q_vector: [100, 100, 500, 0, 0, 0, 0, 0, 0, 0, 10000, 10000, 10000, - 0, 0, 0, 5, 5, 10, 1, 1, 1, 5, 5, 5] -# Penalty on all decision variables -g_vector: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -#g_vector: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -# 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -# Penalty on all decision variables -u_vector: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -#u_vector: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 1, -# 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -# Penalty on efforts, assuming diagonal -r_vector: [1, 1, 1, 0.1, 0.1, 0.1] - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml b/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml deleted file mode 100644 index 8150fd15dd..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml +++ /dev/null @@ -1,71 +0,0 @@ -admm_iter: 2 -rho: 0 # does not do anything -rho_scale: 3.9 -num_threads: 5 -delta_option: 1 -# options are 'MIQP' or 'QP' -projection_type: 'MIQP' -# options are 'stewart_and_trinkle' or 'anitescu' -#contact_model: 'stewart_and_trinkle' -contact_model: 'anitescu' -warm_start: true -use_predicted_x0: true -end_on_qp_step: false -solve_time_filter_alpha: 0.95 # \bar{dt} = (1 - alpha) dt + (alpha) * \bar{dt} -#publish_frequency : 0 -publish_frequency: 0 - -# Workspace Limits (specified as Linear Constraints on the end effector position) -workspace_limits: [[0.866, 0.5, 0.0, 0.3, 0.49], - [-0.5, 0.866, 0.0, -0.1782, -0.1682], - [0.0, 0.0, 1.0, 0.35, 0.7]] -workspace_margins: 0.05 - -u_horizontal_limits: [-10, 10] -u_vertical_limits: [0, 30] - -# LCS generation parameters -mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] -dt: 0.075 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 7 -N: 5 -gamma: 1.0 # discount factor on MPC costs - -#matrix scaling -w_Q: 50 -w_R: 50 -#Penalty on all decision variables, assuming scalar -w_G: 0.15 -#Penalty on all decision variables, assuming scalar -w_U: 0.1 - -#State Tracking Error, assuming diagonal -q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, - 5, 5, 15, 10, 10, 1, 5, 5, 5] -#Penalty on efforts, assuming diagonal -r_vector: [0.15, 0.15, 0.1] - -#Penalty on matching projected variables -g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [1, 1, 1, 1, 1, 1, 1] -g_lambda_n: [1, 1, 1, 1, 1, 1, 1] -g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -g_u: [1, 1, 1] - -#Penalty on matching the QP variables -u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_gamma: [1, 1, 1, 1, 1, 1, 1] -u_lambda_n: [1, 1, 1, 1, 1, 1, 1] -u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -u_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -u_u: [30, 30, 30] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml b/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml deleted file mode 100644 index 33c0fae0b2..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml +++ /dev/null @@ -1,73 +0,0 @@ -admm_iter: 2 -rho: 0 # does not do anything -rho_scale: 4 -num_threads: 5 -delta_option: 1 -#options are 'MIQP' or 'QP' -projection_type: 'MIQP' -#options are 'stewart_and_trinkle' or 'anitescu' -#contact_model : 'stewart_and_trinkle' -contact_model: 'anitescu' -warm_start: true -use_predicted_x0: true -use_robust_formulation: false -end_on_qp_step: false -solve_time_filter_alpha: 0.95 -#set to 0 to publish as fast as possible -publish_frequency: 0 -#publish_frequency: 15 - -# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] -workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], - [0.0, 1.0, 0.0, -0.1, 0.1], - [0.0, 0.0, 1.0, 0.35, 0.7]] - -workspace_margins: 0.05 - -u_horizontal_limits: [-10, 10] -u_vertical_limits: [0, 30] - -mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] -dt: 0.075 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 7 -N: 5 -gamma: 1.0 # discount factor on MPC costs - -#matrix scaling -w_Q: 50 -w_R: 50 -#Penalty on all decision variables, assuming scalar -w_G: 0.15 -#Penalty on all decision variables, assuming scalar -w_U: 0.1 - -#State Tracking Error, assuming diagonal -q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, - 5, 5, 15, 10, 10, 1, 5, 5, 5] -#Penalty on efforts, assuming diagonal -r_vector: [0.15, 0.15, 0.1] - -#Penalty on matching projected variables -g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [1, 1, 1, 1, 1, 1, 1] -g_lambda_n: [1, 1, 1, 1, 1, 1, 1] -g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -g_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] -g_u: [1, 1, 1] - -#Penalty on matching the QP variables -u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_gamma: [1, 1, 1, 1, 1, 1, 1] -u_lambda_n: [1, 1, 1, 1, 1, 1, 1] -u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -u_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] -u_u: [30, 30, 30] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml b/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml deleted file mode 100644 index c504cd699e..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml +++ /dev/null @@ -1,74 +0,0 @@ -admm_iter: 5 -rho: 0 # does not do anything -rho_scale: 2 -num_threads: 5 -delta_option: 1 -#options are 'MIQP' or 'QP' -projection_type: 'MIQP' -#options are 'stewart_and_trinkle' or 'anitescu' -contact_model : 'stewart_and_trinkle' -#contact_model: 'anitescu' -warm_start: true -use_predicted_x0: true -use_robust_formulation: true -end_on_qp_step: false -solve_time_filter_alpha: 0.95 -#set to 0 to publish as fast as possible -publish_frequency : 0 -#publish_frequency: 15 - -# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] -workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], - [0.0, 1.0, 0.0, -0.1, 0.1], - [0.0, 0.0, 1.0, 0.35, 0.7]] -#world_y_limits: [-0.1, 0.1] -#world_z_limits: [0.35, 0.7] -workspace_margins: 0.05 - -u_horizontal_limits: [-10, 10] -u_vertical_limits: [0, 30] - -mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] -dt: 0.075 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 7 -N: 5 -gamma: 1.0 # discount factor on MPC costs - -#matrix scaling -w_Q: 50 -w_R: 50 -#Penalty on all decision variables, assuming scalar -w_G: 0.15 -#Penalty on all decision variables, assuming scalar -w_U: 0.1 - -#State Tracking Error, assuming diagonal -q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, - 5, 5, 15, 10, 10, 1, 5, 5, 5] -#Penalty on efforts, assuming diagonal -r_vector: [0.15, 0.15, 0.1] - -#Penalty on matching projected variables -g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [1, 1, 1, 1, 1, 1, 1] -g_lambda_n: [50, 50, 50, 50, 50, 50, 50] -g_lambda_t: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] -g_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] -g_u: [1, 1, 1] - -#Penalty on matching the QP variables -u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_gamma: [10, 10, 10, 10, 10, 10, 10] -u_lambda_n: [300, 300, 300, 300, 300, 300, 300] -u_lambda_t: [300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300] -u_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] -u_u: [30, 30, 30] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml b/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml deleted file mode 100644 index 45c679466d..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml +++ /dev/null @@ -1,72 +0,0 @@ -admm_iter: 2 -rho: 0.0 -rho_scale: 4 -num_threads: 5 -delta_option: 1 -# options are 'MIQP' or 'QP' -projection_type: 'MIQP' -# options are 'stewart_and_trinkle' or 'anitescu' -#contact_model: 'anitescu' -contact_model: 'stewart_and_trinkle' -warm_start: false -use_predicted_x0: false -use_robust_formulation: false -end_on_qp_step: false -solve_time_filter_alpha: 0.95 -#publish_frequency : 0 -publish_frequency: 0 - -# Workspace Limits -workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], - [0.0, 1.0, 0.0, -0.2, 0.2], - [0.0, 0.0, 1.0, 0.3, 0.6]] -workspace_margins: 0.05 - -u_horizontal_limits: [-50, 50] -u_vertical_limits: [-50, 50] - -mu: [0.6, 0.6, 0.6] -dt: 0.05 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 3 -N: 5 -gamma: 1 - -# matrix scaling -w_Q: 50 -w_R: 25 -# Penalty on all decision variables, assuming scalar -w_G: 0.1 -# Penalty on all decision variables, assuming scalar -w_U: 0.5 - -# State Tracking Error, assuming diagonal -q_vector: [175, 175, 175, 1, 1, 1, 1, 15000, 15000, 15000, - 5, 5, 5, 10, 10, 10, 5, 5, 5] - -# Penalty on efforts, assuming diagonal -r_vector: [0.15, 0.15, 0.1] - -# Penalty on all decision variables -g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [50, 50, 50] -g_lambda_n: [50, 50, 50] -g_lambda_t: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] -g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -g_u: [1, 1, 1] - -# Penalty on all decision variables -u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_gamma: [100, 100, 100] -u_lambda_n: [100, 100, 100] -u_lambda_t: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -u_lambda: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] -u_u: [30, 30, 30] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml b/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml deleted file mode 100644 index a8271bfd81..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml +++ /dev/null @@ -1,71 +0,0 @@ -admm_iter: 2 -rho: 0.0 -rho_scale: 4 -num_threads: 5 -delta_option: 1 -# options are 'MIQP' or 'QP' -projection_type: 'MIQP' -# options are 'stewart_and_trinkle' or 'anitescu' -contact_model: 'anitescu' -#contact_model: 'stewart_and_trinkle' -warm_start: true -use_predicted_x0: false -end_on_qp_step: false -solve_time_filter_alpha: 0.9 -#publish_frequency : 0 -publish_frequency: 25 - -# Workspace Limits -workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], - [0.0, 1.0, 0.0, -0.2, 0.2], - [0.0, 0.0, 1.0, 0.3, 0.6]] -workspace_margins: 0.05 - -u_horizontal_limits: [-10, 10] -u_vertical_limits: [0, 30] - -mu: [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] -dt: 0.05 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 6 -N: 5 -gamma: 1 - -# matrix scaling -w_Q: 50 -w_R: 1 -# Penalty on all decision variables, assuming scalar -w_G: 1 -# Penalty on all decision variables, assuming scalar -w_U: 0.5 - -# State Tracking Error, assuming diagonal -q_vector: [175, 175, 175, 10, 10, 10, 10, 5000, 5000, 5000, - 5, 5, 10, 1, 1, 1, 5, 5, 5] - -# Penalty on efforts, assuming diagonal -r_vector: [0.1, 0.1, 0.1] - -# Penalty on all decision variables -g_x: [1, 1, 1, 1, 1, 1, 1, 10, 10, 10, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [1, 1, 1] -g_lambda_n: [1, 1, 1] -g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -g_u: [1, 1, 1] - -# Penalty on all decision variables -u_x: [1, 1, 1, 1, 1, 1, 1, 10, 10, 10, 1, 1, 1, 10, 10, 10, 10, 10, 10] -u_gamma: [1, 1, 1] -u_lambda_n: [1, 1, 1] -u_lambda_t: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_lambda: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] -u_u: [10, 10, 10] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml b/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml deleted file mode 100644 index fb5f43beb2..0000000000 --- a/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml +++ /dev/null @@ -1,71 +0,0 @@ -admm_iter: 3 -rho: 0 # does not do anything -rho_scale: 5 -num_threads: 4 -delta_option: 1 -#options are 'MIQP' or 'QP' -projection_type: 'MIQP' -#options are 'stewart_and_trinkle' or 'anitescu' -#contact_model : 'stewart_and_trinkle' -contact_model: 'anitescu' -warm_start: true -use_predicted_x0: true -end_on_qp_step: false -solve_time_filter_alpha: 0.95 -#set to 0 to publish as fast as possible -publish_frequency : 0 -#publish_frequency: 25 - -# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] -workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.7], - [0.0, 1.0, 0.0, -0.2, 0.2], - [0.0, 0.0, 1.0, 0.4, 0.5]] -workspace_margins: 0.05 - -u_horizontal_limits: [-10, 10] -u_vertical_limits: [-0, 30] - -mu: [0.8, 0.8, 0.8, 1.0] -dt: 0.05 -solve_dt: 0.05 -num_friction_directions: 2 -num_contacts: 4 -N: 4 -gamma: 1.0 # discount factor on MPC costs - -#matrix scaling -w_Q: 50 -w_R: 75 -#Penalty on all decision variables, assuming scalar -w_G: 0.5 -#Penalty on all decision variables, assuming scalar -w_U: 0.5 - -#State Tracking Error, assuming diagonal -q_vector: [10, 25, 150, 1000, 1000, 1000, 1000, 25, 25, 15000, - 2.5, 5, 5, 1, 1, 500, 5, 5, 5] -#Penalty on efforts, assuming diagonal -r_vector: [2.0, 0.75, 0.05] - -#Penalty on matching projected variables -g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_gamma: [1, 1, 1, 1] -g_lambda_n: [1, 1, 1, 1] -g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -g_lambda: [150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150] -g_u: [2.5, 2.5, 2.5] - -#Penalty on matching the QP variables -u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -u_gamma: [1, 1, 1, 1] -u_lambda_n: [1, 1, 1, 1] -u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] -u_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -u_u: [30, 30, 30] - - -# Parameters used in the QP projection method. As alpha -> 0, any error in -# complimentarity constraints also approaches 0. -qp_projection_alpha: 0.01 -qp_projection_scaling: 1000 -penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml b/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml deleted file mode 100644 index 069fb2bf64..0000000000 --- a/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml +++ /dev/null @@ -1,14 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -end_effector_name: plate -end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf -tool_attachment_frame: [0, 0, 0.107] -end_effector_thickness: 0.016 -object_models: [examples/franka/urdf/tray_lcs.sdf] -environment_models: [examples/franka/urdf/support_point_contact.urdf, - examples/franka/urdf/support_point_contact.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0. , 0. , 0.5236], - [0. , 0. , 0.5236]] -environment_positions: [[0.6178, 0.3299, 0.45 ], - [0.7678, 0.0701, 0.45 ]] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/supports_scene.yaml b/examples/franka/parameters/c3_scenes/supports_scene.yaml deleted file mode 100644 index a0aaecdb2f..0000000000 --- a/examples/franka/parameters/c3_scenes/supports_scene.yaml +++ /dev/null @@ -1,14 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -end_effector_name: plate -end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf -tool_attachment_frame: [0, 0, 0.107] -end_effector_thickness: 0.016 -object_models: [examples/franka/urdf/tray_lcs.sdf] -environment_models: [examples/franka/urdf/support_point_contact.urdf, - examples/franka/urdf/support_point_contact.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0.0, 0.0, 0.0], - [0.0, 0.0, 0.0]] -environment_positions: [[0.8, 0.15, 0.447], - [0.8, -0.15, 0.447]] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/tray_scene.yaml b/examples/franka/parameters/c3_scenes/tray_scene.yaml deleted file mode 100644 index 4d8fae92ac..0000000000 --- a/examples/franka/parameters/c3_scenes/tray_scene.yaml +++ /dev/null @@ -1,11 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -end_effector_name: plate -end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf -tool_attachment_frame: [0, 0, 0.107] -end_effector_thickness: 0.016 -object_models: [examples/franka/urdf/tray_lcs.sdf] -environment_models: [] -# orientation in RPY then position in XYZ -environment_orientations: [] -environment_positions: [] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/two_object_scene.yaml b/examples/franka/parameters/c3_scenes/two_object_scene.yaml deleted file mode 100644 index 9a394e55c8..0000000000 --- a/examples/franka/parameters/c3_scenes/two_object_scene.yaml +++ /dev/null @@ -1,11 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -end_effector_name: plate -end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf -tool_attachment_frame: [0, 0, 0.107] -end_effector_thickness: 0.016 -object_models: [examples/franka/urdf/tray_lcs.sdf, examples/franka/urdf/cylinder_lcs.sdf] -environment_models: [] -# orientation in RPY then position in XYZ -environment_orientations: [] -environment_positions: [] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/wall_scene.yaml b/examples/franka/parameters/c3_scenes/wall_scene.yaml deleted file mode 100644 index 39e02111a1..0000000000 --- a/examples/franka/parameters/c3_scenes/wall_scene.yaml +++ /dev/null @@ -1,11 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -end_effector_name: plate -end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf -tool_attachment_frame: [0, 0, 0.107] -end_effector_thickness: 0.016 -object_models: [examples/franka/urdf/tray_lcs.sdf] -environment_models: [examples/franka/urdf/side_wall.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0.0, 0.0, 0.0]] -environment_positions: [[0.6, 0.285, 0.35]] \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_controller_params.h b/examples/franka/parameters/franka_c3_controller_params.h deleted file mode 100644 index 4b09b48ca6..0000000000 --- a/examples/franka/parameters/franka_c3_controller_params.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include "solvers/c3_options.h" - -#include "drake/common/yaml/yaml_read_archive.h" - -struct FrankaC3ControllerParams { - int scene_index; - std::vector c3_options_file; - std::vector c3_scene_file; - std::string osqp_settings_file; - - bool include_end_effector_orientation; - double target_frequency; - - double near_target_threshold; - std::vector first_target; - std::vector second_target; - std::vector third_target; - double x_scale; - double y_scale; - double z_scale; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(c3_options_file)); - a->Visit(DRAKE_NVP(c3_scene_file)); - a->Visit(DRAKE_NVP(osqp_settings_file)); - - a->Visit(DRAKE_NVP(include_end_effector_orientation)); - a->Visit(DRAKE_NVP(target_frequency)); - a->Visit(DRAKE_NVP(scene_index)); - - a->Visit(DRAKE_NVP(first_target)); - a->Visit(DRAKE_NVP(second_target)); - a->Visit(DRAKE_NVP(third_target)); - a->Visit(DRAKE_NVP(x_scale)); - a->Visit(DRAKE_NVP(y_scale)); - a->Visit(DRAKE_NVP(z_scale)); - a->Visit(DRAKE_NVP(near_target_threshold)); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_controller_params.yaml b/examples/franka/parameters/franka_c3_controller_params.yaml deleted file mode 100644 index 40a59b106a..0000000000 --- a/examples/franka/parameters/franka_c3_controller_params.yaml +++ /dev/null @@ -1,36 +0,0 @@ -scene_index: 1 -c3_options_file: [examples/franka/parameters/c3_options/franka_c3_options_translation.yaml, - examples/franka/parameters/c3_options/franka_c3_options_supports.yaml, - examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml, - examples/franka/parameters/c3_options/franka_c3_options_wall.yaml, - examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml] -c3_scene_file: [examples/franka/parameters/c3_scenes/tray_scene.yaml, - examples/franka/parameters/c3_scenes/supports_scene.yaml, - examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml, - examples/franka/parameters/c3_scenes/wall_scene.yaml, - examples/franka/parameters/c3_scenes/tray_scene.yaml] -osqp_settings_file: examples/franka/parameters/franka_c3_qp_settings.yaml - -include_end_effector_orientation: false -# Note: C3 is set to update much faster than this number. This is just to reduce preempting of the current plan -target_frequency: 0 #unused - -near_target_threshold: 0.05 -first_target: [[0.55, 0.0, 0.485], - [0.45, 0, 0.485], - [0.3897, 0.025, 0.49], - [0.55, 0.0, 0.485], - [0.45, 0.0, 0.5],] -second_target: [[0.55, 0.0, 0.485], - [0.45, 0, 0.6], - [0.3897, 0.025, 0.6], - [0.55, 0.0, 0.485], - [0.45, 0.0, 0.5],] -third_target: [[0.55, 0.0, 0.485], - [0.7, 0.00, 0.485], - [0.6062, 0.15, 0.49], - [0.55, 0.0, 0.485], - [0.45, 0.0, 0.5],] -x_scale: 0.1 -y_scale: 0.1 -z_scale: 0.1 diff --git a/examples/franka/parameters/franka_c3_qp_settings.yaml b/examples/franka/parameters/franka_c3_qp_settings.yaml deleted file mode 100644 index 8a12cbe60e..0000000000 --- a/examples/franka/parameters/franka_c3_qp_settings.yaml +++ /dev/null @@ -1,26 +0,0 @@ -print_to_console: 0 -log_file_name: "" -int_options: - max_iter: 1000 - verbose: 0 - warm_start: 1 - scaling: 1 - adaptive_rho: 1 - adaptive_rho_interval: 0 - polish: 1 - polish_refine_iter: 1 - scaled_termination: 1 - check_termination: 100 -double_options: - rho: 0.1 - sigma: 1e-6 - eps_abs: 1e-5 - eps_rel: 1e-5 - eps_prim_inf: 1e-5 - eps_dual_inf: 1e-5 - alpha: 1.6 - delta: 1e-6 - adaptive_rho_tolerance: 5 - adaptive_rho_fraction: 0.4 - time_limit: 0 -string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_scene_params.h b/examples/franka/parameters/franka_c3_scene_params.h deleted file mode 100644 index 7d4034db74..0000000000 --- a/examples/franka/parameters/franka_c3_scene_params.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include "drake/common/yaml/yaml_read_archive.h" - -struct FrankaC3SceneParams { - std::string franka_model; - std::string end_effector_model; - std::string end_effector_name; - Eigen::Vector3d tool_attachment_frame; - double end_effector_thickness; - std::string end_effector_lcs_model; - std::vector object_models; - std::vector environment_models; - std::vector environment_orientations; - std::vector environment_positions; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(franka_model)); - a->Visit(DRAKE_NVP(end_effector_model)); - a->Visit(DRAKE_NVP(end_effector_name)); - a->Visit(DRAKE_NVP(tool_attachment_frame)); - a->Visit(DRAKE_NVP(end_effector_thickness)); - a->Visit(DRAKE_NVP(end_effector_lcs_model)); - a->Visit(DRAKE_NVP(object_models)); - a->Visit(DRAKE_NVP(environment_models)); - a->Visit(DRAKE_NVP(environment_orientations)); - a->Visit(DRAKE_NVP(environment_positions)); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_lcm_channels.h b/examples/franka/parameters/franka_lcm_channels.h deleted file mode 100644 index adb84c4e2b..0000000000 --- a/examples/franka/parameters/franka_lcm_channels.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include "drake/common/yaml/yaml_read_archive.h" - - -struct FrankaLcmChannels { - std::string franka_state_channel; - std::string tray_state_channel; - std::string object_state_channel; - std::string franka_input_channel; - std::string franka_input_echo; - std::string osc_channel; - std::string osc_debug_channel; - std::string c3_actor_channel; - std::string c3_object_channel; - std::string c3_force_channel; - std::string c3_debug_output_channel; - std::string c3_target_state_channel; - std::string c3_actual_state_channel; - std::string radio_channel; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(franka_state_channel)); - a->Visit(DRAKE_NVP(tray_state_channel)); - a->Visit(DRAKE_NVP(object_state_channel)); - a->Visit(DRAKE_NVP(franka_input_channel)); - a->Visit(DRAKE_NVP(franka_input_echo)); - a->Visit(DRAKE_NVP(osc_channel)); - a->Visit(DRAKE_NVP(osc_debug_channel)); - a->Visit(DRAKE_NVP(c3_actor_channel)); - a->Visit(DRAKE_NVP(c3_object_channel)); - a->Visit(DRAKE_NVP(c3_force_channel)); - a->Visit(DRAKE_NVP(c3_debug_output_channel)); - a->Visit(DRAKE_NVP(c3_target_state_channel)); - a->Visit(DRAKE_NVP(c3_actual_state_channel)); - a->Visit(DRAKE_NVP(radio_channel)); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_controller_params.h b/examples/franka/parameters/franka_osc_controller_params.h deleted file mode 100644 index 2c1b6215f4..0000000000 --- a/examples/franka/parameters/franka_osc_controller_params.h +++ /dev/null @@ -1,100 +0,0 @@ -#pragma once - -#include "systems/controllers/osc/osc_gains.h" - -#include "drake/common/yaml/yaml_read_archive.h" - -struct FrankaControllerParams : OSCGains { - std::string franka_model; - std::string end_effector_model; - std::string end_effector_name; - - Eigen::VectorXd tool_attachment_frame; - double end_effector_acceleration; - bool track_end_effector_orientation; - bool cancel_gravity_compensation; - bool enforce_acceleration_constraints; - bool publish_debug_info; - - Eigen::VectorXd neutral_position; - double x_scale; - double y_scale; - double z_scale; - - double w_elbow; - double elbow_kp; - double elbow_kd; - - std::vector EndEffectorW; - std::vector EndEffectorKp; - std::vector EndEffectorKd; - std::vector EndEffectorRotW; - std::vector EndEffectorRotKp; - std::vector EndEffectorRotKd; - std::vector LambdaEndEffectorW; - - Eigen::MatrixXd W_end_effector; - Eigen::MatrixXd K_p_end_effector; - Eigen::MatrixXd K_d_end_effector; - Eigen::MatrixXd W_mid_link; - Eigen::MatrixXd K_p_mid_link; - Eigen::MatrixXd K_d_mid_link; - Eigen::MatrixXd W_end_effector_rot; - Eigen::MatrixXd K_p_end_effector_rot; - Eigen::MatrixXd K_d_end_effector_rot; - Eigen::MatrixXd W_ee_lambda; - - template - void Serialize(Archive* a) { - OSCGains::Serialize(a); - - a->Visit(DRAKE_NVP(franka_model)); - a->Visit(DRAKE_NVP(end_effector_model)); - a->Visit(DRAKE_NVP(end_effector_name)); - a->Visit(DRAKE_NVP(end_effector_acceleration)); - a->Visit(DRAKE_NVP(track_end_effector_orientation)); - a->Visit(DRAKE_NVP(cancel_gravity_compensation)); - a->Visit(DRAKE_NVP(enforce_acceleration_constraints)); - a->Visit(DRAKE_NVP(publish_debug_info)); - a->Visit(DRAKE_NVP(EndEffectorW)); - a->Visit(DRAKE_NVP(EndEffectorKp)); - a->Visit(DRAKE_NVP(EndEffectorKd)); - a->Visit(DRAKE_NVP(EndEffectorRotW)); - a->Visit(DRAKE_NVP(EndEffectorRotKp)); - a->Visit(DRAKE_NVP(EndEffectorRotKd)); - a->Visit(DRAKE_NVP(LambdaEndEffectorW)); - a->Visit(DRAKE_NVP(w_elbow)); - a->Visit(DRAKE_NVP(elbow_kp)); - a->Visit(DRAKE_NVP(elbow_kd)); - a->Visit(DRAKE_NVP(tool_attachment_frame)); - a->Visit(DRAKE_NVP(neutral_position)); - a->Visit(DRAKE_NVP(x_scale)); - a->Visit(DRAKE_NVP(y_scale)); - a->Visit(DRAKE_NVP(z_scale)); - - W_end_effector = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorW.data(), 3, 3); - K_p_end_effector = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorKp.data(), 3, 3); - K_d_end_effector = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorKd.data(), 3, 3); - W_mid_link = this->w_elbow * MatrixXd::Identity(1, 1); - K_p_mid_link = this->elbow_kp * MatrixXd::Identity(1, 1); - K_d_mid_link = this->elbow_kd * MatrixXd::Identity(1, 1); - W_end_effector_rot = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorRotW.data(), 3, 3); - K_p_end_effector_rot = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorRotKp.data(), 3, 3); - K_d_end_effector_rot = Eigen::Map< - Eigen::Matrix>( - this->EndEffectorRotKd.data(), 3, 3); - W_ee_lambda = Eigen::Map< - Eigen::Matrix>( - this->LambdaEndEffectorW.data(), 3, 3); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_controller_params.yaml b/examples/franka/parameters/franka_osc_controller_params.yaml deleted file mode 100644 index 8fa4ae1673..0000000000 --- a/examples/franka/parameters/franka_osc_controller_params.yaml +++ /dev/null @@ -1,68 +0,0 @@ -controller_frequency: 1000 - -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector_massless.urdf -end_effector_name: plate - -tool_attachment_frame: [0, 0, 0.107] - -neutral_position: [0.55, 0, 0.45] -x_scale: 0.1 -y_scale: 0.1 -z_scale: 0.1 - -w_input: 0.00 -w_input_reg: 0.0 -w_accel: 0.00001 -w_soft_constraint: 0.0 -w_lambda: 0.0 -impact_threshold: 0.000 -impact_tau: 0.000 -mu: 1.0 # unused -end_effector_acceleration: 10 -track_end_effector_orientation: false -cancel_gravity_compensation: false -enforce_acceleration_constraints: false -publish_debug_info: true - -W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] -W_input_reg: [ 1, 1, 1, 1, 1, 1, 10] -W_lambda_c_reg: [ 0.001, 0.001, 0.01, - 0.001, 0.001, 0.01, - 0.001, 0.001, 0.01, - 0.001, 0.001, 0.01 ] -W_lambda_h_reg: [ 0.001, 0.001, 0.001, - 0.001, 0.001, 0.001 ] - -w_elbow: 1 -elbow_kp: 200 -elbow_kd: 10 - -EndEffectorW: - [1, 0, 0, - 0, 1, 0, - 0, 0, 1] -EndEffectorKp: - [ 200, 0, 0, - 0, 200, 0, - 0, 0, 200 ] -EndEffectorKd: - [ 20, 0, 0, - 0, 20, 0, - 0, 0, 20 ] -EndEffectorRotW: - [ 10, 0, 0, - 0, 10, 0, - 0, 0, 10 ] -EndEffectorRotKp: - [ 800, 0, 0, - 0, 800, 0, - 0, 0, 800] -EndEffectorRotKd: - [ 40, 0, 0, - 0, 40, 0, - 0, 0, 40] -LambdaEndEffectorW: - [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1 ] diff --git a/examples/franka/parameters/franka_osc_qp_settings.yaml b/examples/franka/parameters/franka_osc_qp_settings.yaml deleted file mode 100644 index e14875b7a1..0000000000 --- a/examples/franka/parameters/franka_osc_qp_settings.yaml +++ /dev/null @@ -1,26 +0,0 @@ -print_to_console: 0 -log_file_name: "" -int_options: - max_iter: 1000 - verbose: 0 - warm_start: 1 - scaling: 1 - adaptive_rho: 1 - adaptive_rho_interval: 0 - polish: 1 - polish_refine_iter: 1 - scaled_termination: 1 - check_termination: 25 -double_options: - rho: 0.001 - sigma: 1e-6 - eps_abs: 1e-5 - eps_rel: 1e-5 - eps_prim_inf: 1e-5 - eps_dual_inf: 1e-5 - alpha: 1.6 - delta: 1e-6 - adaptive_rho_tolerance: 5 - adaptive_rho_fraction: 0.4 - time_limit: 0 -string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_qp_options.yaml b/examples/franka/parameters/franka_qp_options.yaml deleted file mode 100644 index 9771db4a84..0000000000 --- a/examples/franka/parameters/franka_qp_options.yaml +++ /dev/null @@ -1,4 +0,0 @@ -Q: 2 -R: 0.1 -G: 3 -U: 0 diff --git a/examples/franka/parameters/franka_sim_params.h b/examples/franka/parameters/franka_sim_params.h deleted file mode 100644 index 0835d92e10..0000000000 --- a/examples/franka/parameters/franka_sim_params.h +++ /dev/null @@ -1,81 +0,0 @@ -#pragma once - -#include "drake/common/yaml/yaml_read_archive.h" - -struct FrankaSimParams { - std::vector sim_scene_file; - std::string franka_model; - std::string end_effector_model; - std::string tray_model; - std::string object_model; - - double dt; - double realtime_rate; - double actuator_delay; - double franka_publish_rate; - double tray_publish_rate; - double object_publish_rate; - double visualizer_publish_rate; - - int scene_index; - bool visualize_drake_sim; - bool publish_efforts; - bool publish_object_velocities; - - Eigen::VectorXd q_init_franka; - std::vector q_init_tray; - std::vector q_init_object; - Eigen::VectorXd tool_attachment_frame; - - std::vector world_x_limits; - std::vector world_y_limits; - std::vector world_z_limits; - - std::vector external_force_scaling; - - bool visualize_pose_trace; - bool visualize_center_of_mass_plan; - bool visualize_c3_forces; - bool visualize_c3_object_state; - bool visualize_c3_end_effector_state; - bool visualize_workspace; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(sim_scene_file)); - a->Visit(DRAKE_NVP(franka_model)); - a->Visit(DRAKE_NVP(end_effector_model)); - a->Visit(DRAKE_NVP(tray_model)); - a->Visit(DRAKE_NVP(object_model)); - - a->Visit(DRAKE_NVP(dt)); - a->Visit(DRAKE_NVP(realtime_rate)); - a->Visit(DRAKE_NVP(actuator_delay)); - a->Visit(DRAKE_NVP(franka_publish_rate)); - a->Visit(DRAKE_NVP(tray_publish_rate)); - a->Visit(DRAKE_NVP(object_publish_rate)); - a->Visit(DRAKE_NVP(visualizer_publish_rate)); - - a->Visit(DRAKE_NVP(scene_index)); - a->Visit(DRAKE_NVP(visualize_drake_sim)); - a->Visit(DRAKE_NVP(publish_efforts)); - a->Visit(DRAKE_NVP(publish_object_velocities)); - - a->Visit(DRAKE_NVP(q_init_franka)); - a->Visit(DRAKE_NVP(q_init_tray)); - a->Visit(DRAKE_NVP(q_init_object)); - a->Visit(DRAKE_NVP(tool_attachment_frame)); - - a->Visit(DRAKE_NVP(world_x_limits)); - a->Visit(DRAKE_NVP(world_y_limits)); - a->Visit(DRAKE_NVP(world_z_limits)); - a->Visit(DRAKE_NVP(external_force_scaling)); - - a->Visit(DRAKE_NVP(visualize_pose_trace)); - a->Visit(DRAKE_NVP(visualize_center_of_mass_plan)); - a->Visit(DRAKE_NVP(visualize_c3_forces)); - a->Visit(DRAKE_NVP(visualize_c3_object_state)); - a->Visit(DRAKE_NVP(visualize_c3_end_effector_state)); - a->Visit(DRAKE_NVP(visualize_workspace)); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_sim_params.yaml b/examples/franka/parameters/franka_sim_params.yaml deleted file mode 100644 index b95a39c5b4..0000000000 --- a/examples/franka/parameters/franka_sim_params.yaml +++ /dev/null @@ -1,59 +0,0 @@ -franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf -end_effector_model: examples/franka/urdf/plate_end_effector.urdf -tray_model: examples/franka/urdf/tray.sdf # rigid contact model does not do sticking well -object_model: examples/franka/urdf/cylinder_object.urdf -franka_publish_rate: 1000 -tray_publish_rate: 1000 -object_publish_rate: 1000 -visualizer_publish_rate: 32 -actuator_delay: 0.000 - -scene_index: 1 -visualize_drake_sim: false -publish_efforts: true -publish_object_velocities: true - -tool_attachment_frame: [0, 0, 0.107] - -sim_scene_file: [examples/franka/parameters/sim_scenes/empty_scene.yaml, - examples/franka/parameters/sim_scenes/supports_scene.yaml, - examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml, - examples/franka/parameters/sim_scenes/wall_scene.yaml] - -# Workspace Limits -world_x_limits: [[0.4, 0.6], - [0.4, 0.6], - [0.45, 0.65], - [0.45, 0.65]] -world_y_limits: [[-0.15, 0.15], - [-0.1, 0.1], - [-0.05, 0.3], - [0.45, 0.65]] -world_z_limits: [[0.35, 0.7], - [0.35, 0.7], - [0.35, 0.7], - [0.45, 0.65]] - -external_force_scaling: [10.0, 10.0, 10.0] - -dt: 0.0005 -realtime_rate: 1.0 -q_init_franka: [-1.3, 1.6, 1.6, -2.1, 1.57, 1.62, -0.81] - -q_init_tray: [[1, 0, 0, 0, 0.55, 0.0, 0.465], - [1, 0, 0, 0, 0.7, 0.00, 0.485], - [1. , 0. , 0. , 0. , 0.5889, 0.14 , 0.485], - [0.8775826, 0, 0, 0.4794255, 0.55, 0.02, 0.46]] -# [1, 0, 0, 0, 0.55, 0.0, 0.46]] - -q_init_object: [[1, 0, 0, 0, 0.55, 0.0, 0.0], - [1, 0, 0, 0, 0.7, 0.00, 0.0], - [1. , 0. , 0. , 0. , -0.55, 0.0, 0.0 ], - [1. , 0. , 0. , 0. , -1.55, 0.0, 0.0 ]] - -visualize_workspace: false -visualize_pose_trace: true -visualize_center_of_mass_plan: false -visualize_c3_forces: true -visualize_c3_object_state: true -visualize_c3_end_effector_state: false diff --git a/examples/franka/parameters/franka_sim_scene_params.h b/examples/franka/parameters/franka_sim_scene_params.h deleted file mode 100644 index cc546c6b30..0000000000 --- a/examples/franka/parameters/franka_sim_scene_params.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include "drake/common/yaml/yaml_read_archive.h" - -// Currently this scene only defines static environment obstacles -struct FrankaSimSceneParams { - std::vector environment_models; - std::vector environment_orientations; - std::vector environment_positions; - - Eigen::VectorXd camera_pose; - Eigen::VectorXd camera_target; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(environment_models)); - a->Visit(DRAKE_NVP(environment_orientations)); - a->Visit(DRAKE_NVP(environment_positions)); - DRAKE_DEMAND(environment_models.size() == environment_orientations.size()); - DRAKE_DEMAND(environment_models.size() == environment_positions.size()); - - a->Visit(DRAKE_NVP(camera_pose)); - a->Visit(DRAKE_NVP(camera_target)); - } -}; \ No newline at end of file diff --git a/examples/franka/parameters/lcm_channels_hardware.yaml b/examples/franka/parameters/lcm_channels_hardware.yaml deleted file mode 100644 index a1c12f8099..0000000000 --- a/examples/franka/parameters/lcm_channels_hardware.yaml +++ /dev/null @@ -1,17 +0,0 @@ -franka_state_channel: FRANKA_STATE -tray_state_channel: TRAY_STATE -object_state_channel: BOX_STATE -franka_input_channel: FRANKA_INPUT -franka_input_echo: FRANKA_INPUT_ECHO - -osc_channel: OSC_FRANKA -osc_debug_channel: OSC_DEBUG_FRANKA - -c3_actor_channel: C3_TRAJECTORY_ACTOR -c3_object_channel: C3_TRAJECTORY_TRAY -c3_force_channel: C3_TRAJECTORY_FORCE -c3_debug_output_channel: C3_DEBUG -c3_target_state_channel: C3_TARGET -c3_actual_state_channel: C3_ACTUAL - -radio_channel: RADIO diff --git a/examples/franka/parameters/lcm_channels_simulation.yaml b/examples/franka/parameters/lcm_channels_simulation.yaml deleted file mode 100644 index cb99be428b..0000000000 --- a/examples/franka/parameters/lcm_channels_simulation.yaml +++ /dev/null @@ -1,18 +0,0 @@ -franka_state_channel: FRANKA_STATE_SIMULATION # lcmt_robot_output -tray_state_channel: TRAY_STATE_SIMULATION # lcmt_object_state -object_state_channel: OBJECT_STATE_SIMULATION # lcmt_object_state - -franka_input_channel: FRANKA_INPUT # lcmt_robot_input -franka_input_echo: FRANKA_INPUT_ECHO - -osc_channel: OSC_FRANKA -osc_debug_channel: OSC_DEBUG_FRANKA - -c3_actor_channel: C3_TRAJECTORY_ACTOR # lcmt_timestamped_saved_traj -c3_object_channel: C3_TRAJECTORY_TRAY # lcmt_timestamped_saved_traj -c3_force_channel: C3_FORCES -c3_debug_output_channel: C3_DEBUG -c3_target_state_channel: C3_TARGET # lcmt_c3_state -c3_actual_state_channel: C3_ACTUAL # lcmt_c3_state - -radio_channel: RADIO diff --git a/examples/franka/parameters/sim_scenes/empty_scene.yaml b/examples/franka/parameters/sim_scenes/empty_scene.yaml deleted file mode 100644 index b813f26d31..0000000000 --- a/examples/franka/parameters/sim_scenes/empty_scene.yaml +++ /dev/null @@ -1,7 +0,0 @@ -environment_models: [] -# orientation in RPY then position in XYZ -environment_orientations: [] -environment_positions: [] - -camera_pose: [1.5, 0, 0.6] -camera_target: [0.5, 0, 0.5] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml b/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml deleted file mode 100644 index 6f16b5b21b..0000000000 --- a/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml +++ /dev/null @@ -1,16 +0,0 @@ -environment_models: [examples/franka/urdf/support.urdf, - examples/franka/urdf/support.urdf, - examples/franka/urdf/center_support.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0. , 0. , 0.5236], - [0. , 0. , 0.5236], - [0. , 0. , 0.5236]] -environment_positions: [[0.6178, 0.3299, 0.45 ], - [0.7678, 0.0701, 0.45 ], - [0.5889, 0.14 , 0.45 ]] - -#camera_pose: [ 0.5, -0.9, 0.75 ] -#camera_target: [ 0.5, 0, 0.5 ] - -camera_pose: [ 0.5, 0.0, 1.5 ] -camera_target: [ 0.5, 0, 0.5 ] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/supports_scene.yaml b/examples/franka/parameters/sim_scenes/supports_scene.yaml deleted file mode 100644 index 1f9c99e12a..0000000000 --- a/examples/franka/parameters/sim_scenes/supports_scene.yaml +++ /dev/null @@ -1,13 +0,0 @@ -environment_models: [examples/franka/urdf/support.urdf, - examples/franka/urdf/support.urdf, - examples/franka/urdf/center_support.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0]] -environment_positions: [[0.8, 0.15, 0.447], - [0.8, -0.15, 0.447], - [0.7, 0.0, 0.447]] - -camera_pose: [0.5, -0.9, 0.75] -camera_target: [0.5, 0, 0.5] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/wall_scene.yaml b/examples/franka/parameters/sim_scenes/wall_scene.yaml deleted file mode 100644 index 336dae7191..0000000000 --- a/examples/franka/parameters/sim_scenes/wall_scene.yaml +++ /dev/null @@ -1,10 +0,0 @@ -camera_pose: [ 1.5, 0, 0.75 ] -camera_target: [ 0.5, 0, 0.5 ] -#camera_pose: [ 0.55, 0, 1.2 ] -#camera_target: [ 0.55, 0, 0.5 ] - -environment_models: [examples/franka/urdf/side_wall.urdf] -# orientation in RPY then position in XYZ -environment_orientations: [[0.0, 0.0, 0.0]] -environment_positions: [[0.6, 0.3, 0.35]] - diff --git a/examples/franka/start_logging.py b/examples/franka/start_logging.py deleted file mode 100644 index 0717bbc03c..0000000000 --- a/examples/franka/start_logging.py +++ /dev/null @@ -1,39 +0,0 @@ -import subprocess -import os -import glob -import codecs -from datetime import date -import sys - -def main(log_type): - curr_date = date.today().strftime("%m_%d_%y") - year = date.today().strftime("%Y") - logdir = f"{os.getenv('HOME')}/logs/{year}/{curr_date}" - dair = f"{os.getenv('HOME')}/workspace/dairlib/" - - if not os.path.isdir(logdir): - os.mkdir(logdir) - - git_diff = subprocess.check_output(['git', 'diff'], cwd=dair) - commit_tag = subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=dair) - - os.chdir(logdir) - current_logs = sorted(glob.glob(log_type + 'log-*')) - try: - last_log = int(current_logs[-1].split('-')[-1]) - log_num = f'{last_log+1:02}' - except: - log_num = '00' - - if log_type == 'hw': - with open('commit_tag%s' % log_num, 'w') as f: - f.write(str(commit_tag)) - f.write("\n\ngit diff:\n\n") - f.write(codecs.getdecoder("unicode_escape")(git_diff)[0]) - - subprocess.run(['lcm-logger', '-f', log_type + 'log-%s' % log_num]) - - -if __name__ == '__main__': - log_type = sys.argv[1] - main(log_type) diff --git a/examples/franka/systems/BUILD.bazel b/examples/franka/systems/BUILD.bazel deleted file mode 100644 index 2f2e1e7fc1..0000000000 --- a/examples/franka/systems/BUILD.bazel +++ /dev/null @@ -1,59 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "franka_systems", - srcs = [], - deps = [ - ":c3_trajectory_generator", - ":external_force_generator", - ":plate_balancing_target", - ], -) - -cc_library( - name = "external_force_generator", - srcs = ["external_force_generator.cc"], - hdrs = ["external_force_generator.h"], - deps = [ - "//lcmtypes:lcmt_robot", - "//multibody:utils", - "//systems/framework:vector", - "@drake//:drake_shared_library", - "@lcm", - ], -) - -cc_library( - name = "plate_balancing_target", - srcs = ["plate_balancing_target.cc"], - hdrs = ["plate_balancing_target.h"], - deps = [ - "//lcmtypes:lcmt_robot", - "//multibody:utils", - "//systems/framework:vector", - "@drake//:drake_shared_library", - "@lcm", - ], -) - -cc_library( - name = "c3_trajectory_generator", - srcs = [ - "c3_trajectory_generator.cc", - ], - hdrs = [ - "c3_trajectory_generator.h", - ], - deps = [ - "//systems:franka_kinematics", - "//common:find_resource", - "//lcm:lcm_trajectory_saver", - "//lcmtypes:lcmt_robot", - "//multibody:utils", - "//solvers:c3", - "//solvers:c3_output", - "//solvers:solver_options_io", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) \ No newline at end of file diff --git a/examples/franka/systems/c3_trajectory_generator.cc b/examples/franka/systems/c3_trajectory_generator.cc deleted file mode 100644 index 024223162b..0000000000 --- a/examples/franka/systems/c3_trajectory_generator.cc +++ /dev/null @@ -1,154 +0,0 @@ -#include "examples/franka/systems/c3_trajectory_generator.h" - -#include - -#include "common/find_resource.h" -#include "dairlib/lcmt_timestamped_saved_traj.hpp" -#include "systems/franka_kinematics_vector.h" -#include "multibody/multibody_utils.h" -#include "solvers/c3_output.h" -#include "solvers/lcs.h" - -namespace dairlib { - -using drake::multibody::ModelInstanceIndex; -using drake::systems::BasicVector; -using drake::systems::Context; -using drake::systems::DiscreteValues; -using Eigen::MatrixXd; -using Eigen::MatrixXf; -using Eigen::VectorXd; -using solvers::LCS; -using systems::TimestampedVector; - -namespace systems { - -C3TrajectoryGenerator::C3TrajectoryGenerator( - const drake::multibody::MultibodyPlant& plant, C3Options c3_options) - : plant_(plant), c3_options_(std::move(c3_options)), N_(c3_options_.N) { - this->set_name("c3_trajectory_generator"); - - n_q_ = plant_.num_positions(); - n_v_ = plant_.num_velocities(); - n_x_ = n_q_ + n_v_; - if (c3_options_.contact_model == "stewart_and_trinkle") { - n_lambda_ = - 2 * c3_options_.num_contacts + - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } else if (c3_options_.contact_model == "anitescu") { - n_lambda_ = - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } - n_u_ = plant_.num_actuators(); - - c3_solution_port_ = - this->DeclareAbstractInputPort("c3_solution", - drake::Value()) - .get_index(); - - actor_trajectory_port_ = - this->DeclareAbstractOutputPort( - "c3_actor_trajectory_output", - dairlib::lcmt_timestamped_saved_traj(), - &C3TrajectoryGenerator::OutputActorTrajectory) - .get_index(); - object_trajectory_port_ = - this->DeclareAbstractOutputPort( - "c3_object_trajectory_output", - dairlib::lcmt_timestamped_saved_traj(), - &C3TrajectoryGenerator::OutputObjectTrajectory) - .get_index(); -} - -void C3TrajectoryGenerator::OutputActorTrajectory( - const drake::systems::Context& context, - dairlib::lcmt_timestamped_saved_traj* output_traj) const { - const auto& c3_solution = - this->EvalInputValue(context, c3_solution_port_); - DRAKE_DEMAND(c3_solution->x_sol_.rows() == n_q_ + n_v_); - - MatrixXd knots = MatrixXd::Zero(6, N_); - knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); - knots.bottomRows(3) = - c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); - LcmTrajectory::Trajectory end_effector_traj; - end_effector_traj.traj_name = "end_effector_position_target"; - end_effector_traj.datatypes = - std::vector(knots.rows(), "double"); - end_effector_traj.datapoints = knots; - end_effector_traj.time_vector = c3_solution->time_vector_.cast(); - LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, - "end_effector_position_target", - "end_effector_position_target", false); - - MatrixXd force_samples = c3_solution->u_sol_.cast(); - LcmTrajectory::Trajectory force_traj; - force_traj.traj_name = "end_effector_force_target"; - force_traj.datatypes = - std::vector(force_samples.rows(), "double"); - force_traj.datapoints = force_samples; - force_traj.time_vector = c3_solution->time_vector_.cast(); - lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); - - if (publish_end_effector_orientation_) { - LcmTrajectory::Trajectory end_effector_orientation_traj; - // first 3 rows are rpy, last 3 rows are angular velocity - MatrixXd orientation_samples = MatrixXd::Zero(6, N_); - orientation_samples.topRows(3) = - c3_solution->x_sol_.topRows(6).bottomRows(3).cast(); - orientation_samples.bottomRows(3) = c3_solution->x_sol_.bottomRows(n_v_) - .topRows(6) - .bottomRows(3) - .cast(); - end_effector_orientation_traj.traj_name = "end_effector_orientation_target"; - end_effector_orientation_traj.datatypes = - std::vector(orientation_samples.rows(), "double"); - end_effector_orientation_traj.datapoints = orientation_samples; - end_effector_orientation_traj.time_vector = - c3_solution->time_vector_.cast(); - lcm_traj.AddTrajectory(end_effector_orientation_traj.traj_name, - end_effector_orientation_traj); - } - - output_traj->saved_traj = lcm_traj.GenerateLcmObject(); - output_traj->utime = context.get_time() * 1e6; -} - -void C3TrajectoryGenerator::OutputObjectTrajectory( - const drake::systems::Context& context, - dairlib::lcmt_timestamped_saved_traj* output_traj) const { - const auto& c3_solution = - this->EvalInputValue(context, c3_solution_port_); - DRAKE_DEMAND(c3_solution->x_sol_.rows() == n_q_ + n_v_); - MatrixXd knots = MatrixXd::Zero(6, N_); - knots.topRows(3) = c3_solution->x_sol_.middleRows(n_q_ - 3, 3).cast(); - knots.bottomRows(3) = - c3_solution->x_sol_.middleRows(n_q_ + n_v_ - 3, 3).cast(); - LcmTrajectory::Trajectory object_traj; - object_traj.traj_name = "object_position_target"; - object_traj.datatypes = std::vector(knots.rows(), "double"); - object_traj.datapoints = knots; - object_traj.time_vector = c3_solution->time_vector_.cast(); - LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, - "object_target", "object_target", false); - - LcmTrajectory::Trajectory object_orientation_traj; - // first 3 rows are rpy, last 3 rows are angular velocity - MatrixXd orientation_samples = MatrixXd::Zero(4, N_); - orientation_samples = - c3_solution->x_sol_.middleRows(n_q_ - 7, 4).cast(); - object_orientation_traj.traj_name = "object_orientation_target"; - object_orientation_traj.datatypes = - std::vector(orientation_samples.rows(), "double"); - object_orientation_traj.datapoints = orientation_samples; - object_orientation_traj.time_vector = - c3_solution->time_vector_.cast(); - lcm_traj.AddTrajectory(object_orientation_traj.traj_name, - object_orientation_traj); - - output_traj->saved_traj = lcm_traj.GenerateLcmObject(); - output_traj->utime = context.get_time() * 1e6; -} - -} // namespace systems -} // namespace dairlib diff --git a/examples/franka/systems/c3_trajectory_generator.h b/examples/franka/systems/c3_trajectory_generator.h deleted file mode 100644 index 5ae42e5b8a..0000000000 --- a/examples/franka/systems/c3_trajectory_generator.h +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "common/find_resource.h" -#include "dairlib/lcmt_saved_traj.hpp" -#include "dairlib/lcmt_timestamped_saved_traj.hpp" -#include "lcm/lcm_trajectory.h" -#include "solvers/c3_options.h" - -#include "drake/systems/framework/leaf_system.h" - -namespace dairlib { -namespace systems { - -/// Outputs a lcmt_timestamped_saved_traj -class C3TrajectoryGenerator : public drake::systems::LeafSystem { - public: - explicit C3TrajectoryGenerator( - const drake::multibody::MultibodyPlant& plant, - C3Options c3_options); - - const drake::systems::InputPort& get_input_port_c3_solution() const { - return this->get_input_port(c3_solution_port_); - } - - const drake::systems::OutputPort& get_output_port_actor_trajectory() - const { - return this->get_output_port(actor_trajectory_port_); - } - const drake::systems::OutputPort& get_output_port_object_trajectory() - const { - return this->get_output_port(object_trajectory_port_); - } - - void SetPublishEndEffectorOrientation(bool publish_end_effector_orientation) { - publish_end_effector_orientation_ = publish_end_effector_orientation; - } - - private: - void OutputActorTrajectory( - const drake::systems::Context& context, - dairlib::lcmt_timestamped_saved_traj* output_traj) const; - - void OutputObjectTrajectory( - const drake::systems::Context& context, - dairlib::lcmt_timestamped_saved_traj* output_traj) const; - - drake::systems::InputPortIndex c3_solution_port_; - drake::systems::OutputPortIndex actor_trajectory_port_; - drake::systems::OutputPortIndex object_trajectory_port_; - - const drake::multibody::MultibodyPlant& plant_; - C3Options c3_options_; - - bool publish_end_effector_orientation_ = false; - - // convenience for variable sizes - int n_q_; - int n_v_; - int n_x_; - int n_lambda_; - int n_u_; - - int N_; -}; - -} // namespace systems -} // namespace dairlib diff --git a/examples/franka/systems/external_force_generator.cc b/examples/franka/systems/external_force_generator.cc deleted file mode 100644 index f3168085e6..0000000000 --- a/examples/franka/systems/external_force_generator.cc +++ /dev/null @@ -1,62 +0,0 @@ -#include "external_force_generator.h" - -using Eigen::Vector3d; -using Eigen::VectorXd; - -using drake::systems::BasicVector; -using drake::systems::Context; -using drake::systems::EventStatus; - -namespace dairlib { - -ExternalForceGenerator::ExternalForceGenerator( - drake::multibody::BodyIndex body_index) - : body_index_(body_index) { - // Input/Output Setup - radio_port_ = - this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) - .get_index(); - - spatial_force_port_ = - this->DeclareAbstractOutputPort( - "object_spatial_force", - std::vector< - drake::multibody::ExternallyAppliedSpatialForce>(), - &ExternalForceGenerator::CalcSpatialForce) - .get_index(); -} - -void ExternalForceGenerator::SetRemoteControlParameters(double x_scale, - double y_scale, - double z_scale) { - x_scale_ = x_scale; - y_scale_ = y_scale; - z_scale_ = z_scale; -} - -void ExternalForceGenerator::CalcSpatialForce( - const drake::systems::Context& context, - std::vector>* - spatial_forces) const { - const auto& radio_out = this->EvalVectorInput(context, radio_port_); - Vector3d force = VectorXd::Zero(3); - if (radio_out->value()[12]) { - force(0) = radio_out->value()[0] * x_scale_; - force(1) = radio_out->value()[1] * y_scale_; - force(2) = radio_out->value()[2] * z_scale_; - } - if (spatial_forces->empty()) { - auto spatial_force = - drake::multibody::ExternallyAppliedSpatialForce(); - spatial_force.body_index = body_index_; - spatial_force.F_Bq_W = - drake::multibody::SpatialForce(Vector3d::Zero(), force); - spatial_forces->push_back(spatial_force); - } else { - spatial_forces->at(0).F_Bq_W = - drake::multibody::SpatialForce(Vector3d::Zero(), force); - spatial_forces->at(0).body_index = body_index_; - } -} - -} // namespace dairlib diff --git a/examples/franka/systems/external_force_generator.h b/examples/franka/systems/external_force_generator.h deleted file mode 100644 index a94f023f4b..0000000000 --- a/examples/franka/systems/external_force_generator.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#include - -#include "drake/common/trajectories/piecewise_polynomial.h" -#include "drake/systems/framework/leaf_system.h" - -namespace dairlib { - -class ExternalForceGenerator : public drake::systems::LeafSystem { - public: - ExternalForceGenerator(drake::multibody::BodyIndex body_index); - - const drake::systems::InputPort& get_input_port_radio() const { - return this->get_input_port(radio_port_); - } - const drake::systems::OutputPort& get_output_port_spatial_force() - const { - return this->get_output_port(spatial_force_port_); - } - - void SetRemoteControlParameters(double x_scale, double y_scale, - double z_scale); - - private: - void CalcSpatialForce(const drake::systems::Context& context, - std::vector>* spatial_forces) const; - - drake::systems::InputPortIndex radio_port_; - drake::systems::OutputPortIndex spatial_force_port_; - drake::multibody::BodyIndex body_index_; - double x_scale_ = 0; - double y_scale_ = 0; - double z_scale_ = 0; -}; - -} // namespace dairlib diff --git a/examples/franka/systems/plate_balancing_target.cc b/examples/franka/systems/plate_balancing_target.cc deleted file mode 100644 index a503d18af1..0000000000 --- a/examples/franka/systems/plate_balancing_target.cc +++ /dev/null @@ -1,199 +0,0 @@ -#include "plate_balancing_target.h" - -#include -#include - -#include "dairlib/lcmt_radio_out.hpp" - -using dairlib::systems::StateVector; -using drake::multibody::MultibodyPlant; -using drake::systems::BasicVector; -using drake::systems::EventStatus; -using Eigen::VectorXd; - -namespace dairlib { -namespace systems { - -PlateBalancingTargetGenerator::PlateBalancingTargetGenerator( - const MultibodyPlant& object_plant, double end_effector_thickness, - double target_threshold) - : end_effector_thickness_(end_effector_thickness), - target_threshold_(target_threshold) { - // Input/Output Setup - radio_port_ = - this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) - .get_index(); - tray_state_port_ = - this->DeclareVectorInputPort( - "x_object", StateVector(object_plant.num_positions(), - object_plant.num_velocities())) - .get_index(); - end_effector_target_port_ = - this->DeclareVectorOutputPort( - "end_effector_target", BasicVector(3), - &PlateBalancingTargetGenerator::CalcEndEffectorTarget) - .get_index(); - tray_target_port_ = this->DeclareVectorOutputPort( - "tray_target", BasicVector(7), - &PlateBalancingTargetGenerator::CalcTrayTarget) - .get_index(); - tray_velocity_target_port_ = this->DeclareVectorOutputPort( - "tray_velocity_target", BasicVector(6), - &PlateBalancingTargetGenerator::CalcTrayVelocityTarget) - .get_index(); - sequence_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); - within_target_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); - time_entered_target_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); - DeclareForcedDiscreteUpdateEvent( - &PlateBalancingTargetGenerator::DiscreteVariableUpdate); -} - -EventStatus PlateBalancingTargetGenerator::DiscreteVariableUpdate( - const drake::systems::Context& context, - drake::systems::DiscreteValues* discrete_state) const { - const StateVector* tray_state = - (StateVector*)this->EvalVectorInput(context, tray_state_port_); - const auto& radio_out = this->EvalVectorInput(context, radio_port_); - - // Ugly FSM - int current_sequence = context.get_discrete_state(sequence_index_)[0]; - int within_target = context.get_discrete_state(within_target_index_)[0]; - int time_entered_target = - context.get_discrete_state(time_entered_target_index_)[0]; - if (current_sequence == 0) { - if ((tray_state->GetPositions().tail(3) - first_target_).norm() < - target_threshold_) { - if (within_target == - 0) { // set the time of when the tray first hits the target - discrete_state->get_mutable_value(time_entered_target_index_)[0] = - context.get_time(); - } - discrete_state->get_mutable_value(within_target_index_)[0] = 1; - } - if (within_target == 1 && - (context.get_time() - time_entered_target) > 0.5) { - discrete_state->get_mutable_value(within_target_index_)[0] = 0; - discrete_state->get_mutable_value(sequence_index_)[0] = 1; - } - } else if (current_sequence == 1) { - if ((tray_state->GetPositions().tail(3) - second_target_).norm() < - target_threshold_) { - if (within_target == - 0) { // set the time of when the tray first hits the target - discrete_state->get_mutable_value(time_entered_target_index_)[0] = - context.get_time(); - } - discrete_state->get_mutable_value(within_target_index_)[0] = 1; - } - if (within_target == 1 && - (context.get_time() - time_entered_target) > delay_at_top_) { - discrete_state->get_mutable_value(within_target_index_)[0] = 0; - discrete_state->get_mutable_value(sequence_index_)[0] = 2; - } - } else if (current_sequence == 2) { - if ((tray_state->GetPositions().tail(3) - third_target_).norm() < - target_threshold_) { - discrete_state->get_mutable_value(sequence_index_)[0] = 3; - } - } - if (current_sequence == 3 && radio_out->value()[15] < 0) { - discrete_state->get_mutable_value(sequence_index_)[0] = 0; - } - return EventStatus::Succeeded(); -} - -void PlateBalancingTargetGenerator::SetRemoteControlParameters( - const Eigen::Vector3d& first_target, const Eigen::Vector3d& second_target, - const Eigen::Vector3d& third_target, double x_scale, double y_scale, - double z_scale) { - first_target_ = first_target; - second_target_ = second_target; - third_target_ = third_target; - x_scale_ = x_scale; - y_scale_ = y_scale; - z_scale_ = z_scale; -} - -void PlateBalancingTargetGenerator::CalcEndEffectorTarget( - const drake::systems::Context& context, - drake::systems::BasicVector* target) const { - const auto& radio_out = this->EvalVectorInput(context, radio_port_); - - VectorXd end_effector_position = first_target_; - // Update target if remote trigger is active - if (context.get_discrete_state(sequence_index_)[0] == 1) { - end_effector_position = second_target_; // raise the tray once it is close - } else if (context.get_discrete_state(sequence_index_)[0] == 2 || - context.get_discrete_state(sequence_index_)[0] == 3) { - end_effector_position = third_target_; // put the tray back - } - end_effector_position[2] -= - end_effector_thickness_; // place end effector below tray - if (radio_out->value()[13] > 0) { - end_effector_position(0) += radio_out->value()[0] * x_scale_; - end_effector_position(1) += radio_out->value()[1] * y_scale_; - end_effector_position(2) += radio_out->value()[2] * z_scale_; - } - if (end_effector_position[0] > 0.6) { - end_effector_position[0] = 0.6; // keep it within the workspace - } - // end_effector_position(0) = 0.55; - // end_effector_position(1) = 0.1 * sin(4 * context.get_time()); - // end_effector_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()) - - // end_effector_thickness_; end_effector_position(1) = 0.1 * (int) (2 * - // sin(0.5 * context.get_time())); end_effector_position(2) = 0.45 - - // end_effector_thickness_; - target->SetFromVector(end_effector_position); -} - -void PlateBalancingTargetGenerator::CalcTrayTarget( - const drake::systems::Context& context, - BasicVector* target) const { - const auto& radio_out = this->EvalVectorInput(context, radio_port_); - VectorXd target_tray_state = VectorXd::Zero(7); - VectorXd tray_position = first_target_; - - if (context.get_discrete_state(sequence_index_)[0] == 1) { - tray_position = second_target_; - } else if (context.get_discrete_state(sequence_index_)[0] == 2 || - context.get_discrete_state(sequence_index_)[0] == 3) { - tray_position = third_target_; - } - if (radio_out->value()[13] > 0) { - tray_position(0) += radio_out->value()[0] * x_scale_; - tray_position(1) += radio_out->value()[1] * y_scale_; - tray_position(2) += radio_out->value()[2] * z_scale_; - } - // tray_position(0) = 0.55; - // tray_position(1) = 0.1 * sin(4 * context.get_time()); - // tray_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()); - // tray_position(1) = 0.1 * (int) (2 * sin(0.5 * context.get_time())); - // tray_position(2) = 0.45; - target_tray_state << 1, 0, 0, 0, tray_position; // tray orientation is flat - target->SetFromVector(target_tray_state); -} - -void PlateBalancingTargetGenerator::CalcTrayVelocityTarget( - const drake::systems::Context& context, - BasicVector* target) const { - const StateVector* tray_state = - (StateVector*)this->EvalVectorInput(context, tray_state_port_); - Eigen::Quaterniond y_quat_des(1, 0, 0, 0); - const VectorX &q = tray_state->GetPositions(); - Eigen::Quaterniond y_quat(q(0), q(1), q(2), q(3)); - Eigen::AngleAxis angle_axis_diff(y_quat_des * y_quat.inverse()); - VectorXd angle_error = angle_axis_diff.angle() * angle_axis_diff.axis(); - - VectorXd target_tray_state = VectorXd::Zero(6); - - // tray_position(0) = 0.55; - // tray_position(1) = 0.1 * sin(4 * context.get_time()); - // tray_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()); - // tray_position(1) = 0.1 * (int) (2 * sin(0.5 * context.get_time())); - // tray_position(2) = 0.45; - target_tray_state << angle_error, VectorXd::Zero(3); // tray orientation is flat - target->SetFromVector(target_tray_state); -} - -} // namespace systems -} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/systems/plate_balancing_target.h b/examples/franka/systems/plate_balancing_target.h deleted file mode 100644 index be73d052b1..0000000000 --- a/examples/franka/systems/plate_balancing_target.h +++ /dev/null @@ -1,81 +0,0 @@ -#pragma once - -#include - -#include "systems/framework/state_vector.h" - -#include "drake/systems/framework/leaf_system.h" - -namespace dairlib { -namespace systems { - -class PlateBalancingTargetGenerator - : public drake::systems::LeafSystem { - public: - PlateBalancingTargetGenerator( - const drake::multibody::MultibodyPlant& object_plant, - double end_effector_thickness, double target_threshold = 0.075); - - const drake::systems::InputPort& get_input_port_radio() const { - return this->get_input_port(radio_port_); - } - - const drake::systems::InputPort& get_input_port_tray_state() const { - return this->get_input_port(tray_state_port_); - } - - const drake::systems::OutputPort& - get_output_port_end_effector_target() const { - return this->get_output_port(end_effector_target_port_); - } - - const drake::systems::OutputPort& get_output_port_tray_target() - const { - return this->get_output_port(tray_target_port_); - } - - const drake::systems::OutputPort& get_output_port_tray_velocity_target() - const { - return this->get_output_port(tray_velocity_target_port_); - } - - void SetRemoteControlParameters(const Eigen::Vector3d& first_target, - const Eigen::Vector3d& second_target, - const Eigen::Vector3d& third_target, - double x_scale, double y_scale, - double z_scale); - - private: - void CalcEndEffectorTarget(const drake::systems::Context& context, - drake::systems::BasicVector* target) const; - void CalcTrayTarget(const drake::systems::Context& context, - drake::systems::BasicVector* target) const; - void CalcTrayVelocityTarget(const drake::systems::Context& context, - drake::systems::BasicVector* target) const; - drake::systems::EventStatus DiscreteVariableUpdate( - const drake::systems::Context& context, - drake::systems::DiscreteValues* discrete_state) const; - - drake::systems::InputPortIndex radio_port_; - drake::systems::InputPortIndex tray_state_port_; - drake::systems::OutputPortIndex end_effector_target_port_; - drake::systems::OutputPortIndex tray_target_port_; - drake::systems::OutputPortIndex tray_velocity_target_port_; - - drake::systems::DiscreteStateIndex sequence_index_; - drake::systems::DiscreteStateIndex within_target_index_; - drake::systems::DiscreteStateIndex time_entered_target_index_; - double end_effector_thickness_; - Eigen::Vector3d first_target_; - Eigen::Vector3d second_target_; - Eigen::Vector3d third_target_; -// const double delay_at_top_ = 10.0; - const double delay_at_top_ = 3.0; - double target_threshold_; - double x_scale_; - double y_scale_; - double z_scale_; -}; - -} // namespace systems -} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/urdf/center_support.urdf b/examples/franka/urdf/center_support.urdf deleted file mode 100644 index 90ace69be8..0000000000 --- a/examples/franka/urdf/center_support.urdf +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - cod - - - - - - - - - - - cod - - - - - - - diff --git a/examples/franka/urdf/cylinder_lcs.urdf b/examples/franka/urdf/cylinder_lcs.urdf deleted file mode 100644 index 31a4718b6c..0000000000 --- a/examples/franka/urdf/cylinder_lcs.urdf +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/cylinder_object.urdf b/examples/franka/urdf/cylinder_object.urdf deleted file mode 100644 index 5832f2b11b..0000000000 --- a/examples/franka/urdf/cylinder_object.urdf +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/plate_end_effector.urdf b/examples/franka/urdf/plate_end_effector.urdf deleted file mode 100644 index a0efe0cef0..0000000000 --- a/examples/franka/urdf/plate_end_effector.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/plate_end_effector_floating.urdf b/examples/franka/urdf/plate_end_effector_floating.urdf deleted file mode 100644 index efaa4bb17f..0000000000 --- a/examples/franka/urdf/plate_end_effector_floating.urdf +++ /dev/null @@ -1,119 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - 1 - - - - - 1 - - - - - 1 - - - - - 1 - - - - - 1 - - diff --git a/examples/franka/urdf/plate_end_effector_massless.urdf b/examples/franka/urdf/plate_end_effector_massless.urdf deleted file mode 100644 index b81d96de63..0000000000 --- a/examples/franka/urdf/plate_end_effector_massless.urdf +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf b/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf deleted file mode 100644 index 18c5c1a817..0000000000 --- a/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/plate_end_effector_translation.urdf b/examples/franka/urdf/plate_end_effector_translation.urdf deleted file mode 100644 index bb2ca2f4f8..0000000000 --- a/examples/franka/urdf/plate_end_effector_translation.urdf +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - 1 - - - - - 1 - - diff --git a/examples/franka/urdf/plate_end_effector_translation_he.urdf b/examples/franka/urdf/plate_end_effector_translation_he.urdf deleted file mode 100644 index 01d1b5a3d9..0000000000 --- a/examples/franka/urdf/plate_end_effector_translation_he.urdf +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - 1 - - - - - 1 - - diff --git a/examples/franka/urdf/side_wall.urdf b/examples/franka/urdf/side_wall.urdf deleted file mode 100644 index 4cb74faec7..0000000000 --- a/examples/franka/urdf/side_wall.urdf +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - cod - - - - - - - - - - - cod - - - - - - - diff --git a/examples/franka/urdf/support.urdf b/examples/franka/urdf/support.urdf deleted file mode 100644 index 0bfd702a4f..0000000000 --- a/examples/franka/urdf/support.urdf +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/support_point_contact.urdf b/examples/franka/urdf/support_point_contact.urdf deleted file mode 100644 index 3e06df6c64..0000000000 --- a/examples/franka/urdf/support_point_contact.urdf +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/franka/urdf/tray.sdf b/examples/franka/urdf/tray.sdf deleted file mode 100644 index b774f23a7b..0000000000 --- a/examples/franka/urdf/tray.sdf +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - 0 0 0 0 0 0 - 1 - - - 0.013105 - 0 - 0 - 0.013105 - 0 - 0.026129 - - - - 0 0 0.0 0 0 0 - - - 0.2286 - 0.022 - - - - 0.1 0.1 0.1 0.6 - - - - 0 0 0.0 0 0 0 - - - 0.2286 - 0.022 - - - - - 3.0e7 - 0.15 - 10 - 0.4 - - - - - \ No newline at end of file diff --git a/examples/franka/urdf/tray_lcs.sdf b/examples/franka/urdf/tray_lcs.sdf deleted file mode 100644 index 0f5862fe11..0000000000 --- a/examples/franka/urdf/tray_lcs.sdf +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - 0 0 0 0 0 0 - 1 - - - 0.013105 - 0 - 0 - 0.013105 - 0 - 0.026129 - - - - 0 0 0.0 0 0 0 - - - 0.2286 - 0.022 - - - - 0.1 0.1 0.1 0.8 - - - - 0 0 0.0 0 0 0 - - - 0.2286 - 0.022 - - - 0.15 0.3 0 0 0 0 - - - - \ No newline at end of file diff --git a/examples/franka/urdf/tray_transparent.sdf b/examples/franka/urdf/tray_transparent.sdf deleted file mode 100644 index c0947bf6fb..0000000000 --- a/examples/franka/urdf/tray_transparent.sdf +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - 0 0 0 0 0 0 - 1 - - - 0.013105 - 0 - 0 - 0.013105 - 0 - 0.026129 - - - - 0 0 0.0 0 0 0 - - - 0.2276 - 0.022 - - - - 0.1 0.1 0.1 0.9 - - - - 0 0 0.0 0 0 0 - - - 0.2286 - 0.022 - - - - - 3.0e7 - 0.18 - 10 - 0.4 - - - - - \ No newline at end of file diff --git a/examples/sampling_c3/BUILD.bazel b/examples/sampling_c3/BUILD.bazel index 60f6f83e92..8c0f98f00f 100644 --- a/examples/sampling_c3/BUILD.bazel +++ b/examples/sampling_c3/BUILD.bazel @@ -139,6 +139,7 @@ cc_binary( "//systems/trajectory_optimization:lcm_trajectory_systems", "//systems/visualization:lcm_visualization_systems", "@drake//:drake_shared_library", + "@c3//:libc3", "@gflags", ], linkopts = [ @@ -221,7 +222,6 @@ cc_binary( "//systems:system_utils", "//systems/controllers:sampling_c3_controller", "//systems/framework:lcm_driven_loop", - "//systems/trajectory_optimization:c3_output_systems", "@drake//:drake_shared_library", "@gflags", ], @@ -272,7 +272,6 @@ cc_library( "//multibody:geom_geom_collider", "//common:math_utils", "//common:update_context", - "//solvers:c3", ], ) @@ -281,7 +280,6 @@ cc_library( srcs = ["reposition.cc"], hdrs = ["reposition.h"], deps = [ - "//solvers:c3", "//examples/sampling_c3/parameter_headers:sampling_c3_options", "//examples/sampling_c3/parameter_headers:reposition_params", "//common:math_utils", diff --git a/examples/sampling_c3/anything/BUILD.bazel b/examples/sampling_c3/anything/BUILD.bazel index 9d1c9824b8..5b6d685a51 100644 --- a/examples/sampling_c3/anything/BUILD.bazel +++ b/examples/sampling_c3/anything/BUILD.bazel @@ -44,7 +44,6 @@ cc_library( "//examples/sampling_c3:shared_params_yamls", ], deps = [ - "//solvers:c3", "@drake//:drake_shared_library", ], ) @@ -57,7 +56,6 @@ cc_library( ], deps = [ "//examples/sampling_c3/parameter_headers:goal_params", - "//solvers:c3", "@drake//:drake_shared_library", ], ) @@ -70,7 +68,6 @@ cc_library( ], deps = [ "//common/parameters:franka_drake_lcm_driver_channels", - "//solvers:c3", "@drake//:drake_shared_library", ], ) diff --git a/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml b/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml index aa3a522f95..e932638426 100644 --- a/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml +++ b/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml @@ -123,7 +123,6 @@ use_predicted_x0: false # instead: use_predicted_x0_c3, dt: 0 # instead: planning_dt_pose, planning_dt_position dt_cost: 0 solve_dt: 0 # unused -mu: [] # instead based on indexing into mu_per_pair_type num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists # Instead for the below, index into their _list versions. g_gamma: [] diff --git a/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml b/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml index 85e3f60532..9fca21b81a 100644 --- a/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml +++ b/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml @@ -158,7 +158,6 @@ u_eta_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, dt: 0 # instead: planning_dt_pose, planning_dt_position # solve_dt: 0 # unused dt_cost: 0 -mu: [] # instead based on indexing into mu_per_pair_type num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists # Instead for the below, index into their _list versions. g_gamma: [] diff --git a/examples/sampling_c3/franka_sampling_c3_controller.cc b/examples/sampling_c3/franka_sampling_c3_controller.cc index f7fae973da..9f8c65e8ce 100644 --- a/examples/sampling_c3/franka_sampling_c3_controller.cc +++ b/examples/sampling_c3/franka_sampling_c3_controller.cc @@ -10,13 +10,15 @@ #include #include +#include "c3/multibody/lcs_factory.h" +#include "c3/systems/lcmt_generators/c3_output_generator.h" +#include "c3/systems/lcmt_generators/contact_force_generator.h" #include "common/eigen_utils.h" -#include "goal_generator.h" -#include "examples/sampling_c3/sampling_c3_utils.h" -#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" #include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "goal_generator.h" #include "multibody/multibody_utils.h" -#include "solvers/lcs_factory.h" #include "systems/controllers/sampling_based_c3_controller.h" #include "systems/framework/lcm_driven_loop.h" #include "systems/franka_kinematics.h" @@ -24,29 +26,30 @@ #include "systems/senders/c3_state_sender.h" #include "systems/senders/sample_buffer_sender.h" #include "systems/system_utils.h" -#include "systems/trajectory_optimization/c3_output_systems.h" namespace dairlib { -using dairlib::solvers::LCSFactory; +using c3::multibody::LCSFactory; +using c3::systems::lcmt_generators::C3OutputGenerator; +using c3::systems::lcmt_generators::ContactForceGenerator; using drake::SortedPair; using drake::geometry::GeometryId; using drake::math::RigidTransform; using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::ModelInstanceIndex; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; using drake::systems::Diagram; using drake::systems::DiagramBuilder; +using drake::systems::InputPort; +using drake::systems::InputPortIndex; +using drake::systems::OutputPort; +using drake::systems::OutputPortIndex; using drake::systems::TriggerType; using drake::systems::TriggerTypeSet; using drake::systems::lcm::LcmPublisherSystem; using drake::systems::lcm::LcmSubscriberSystem; -using drake::systems::InputPort; -using drake::systems::OutputPort; -using drake::systems::InputPortIndex; -using drake::systems::OutputPortIndex; using Eigen::MatrixXd; -using drake::multibody::ModelInstanceIndex; using Eigen::Vector3d; using Eigen::VectorXd; @@ -54,7 +57,6 @@ using multibody::MakeNameToPositionsMap; using multibody::MakeNameToVelocitiesMap; using std::vector; - DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", "LCM URL with IP, port, and TTL settings"); @@ -66,18 +68,20 @@ int DoMain(int argc, char* argv[]) { drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); // Load parameters. - std::string controller_params_path = "examples/sampling_c3/" + - FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + std::string controller_params_path = + "examples/sampling_c3/" + FLAGS_demo_name + + "/parameters/sampling_c3_controller_params.yaml"; SamplingC3ControllerParams controller_params = drake::yaml::LoadYamlFile( controller_params_path); - std::string lcm_channels_file = FLAGS_is_simulation ? - controller_params.lcm_channels_simulation_file : - controller_params.lcm_channels_hardware_file; + std::string lcm_channels_file = + FLAGS_is_simulation ? controller_params.lcm_channels_simulation_file + : controller_params.lcm_channels_hardware_file; SamplingC3LcmChannels lcm_channel_params = drake::yaml::LoadYamlFile(lcm_channels_file); - SamplingC3Options sampling_c3_options = - drake::yaml::LoadYamlFile(controller_params.sampling_c3_options_file); + SamplingC3Options sampling_c3_options = + drake::yaml::LoadYamlFile( + controller_params.sampling_c3_options_file); // Create a Franka-only plant (no need to add walls to this). MultibodyPlant plant_franka(0.0); @@ -88,25 +92,24 @@ int DoMain(int argc, char* argv[]) { // Create an object-only plant. MultibodyPlant plant_object(0.0); std::vector object_indices = AddObjectsToPlant( - &plant_object, nullptr, controller_params.object_models); - - // exclude ee and ground - //std::vector object_indices(full_object_indices.begin()+2, full_object_indices.end()); + &plant_object, nullptr, controller_params.object_models); + + // exclude ee and ground + // std::vector + // object_indices(full_object_indices.begin()+2, full_object_indices.end()); plant_object.Finalize(); auto object_context = plant_object.CreateDefaultContext(); // Create the LCS plant containing a floating EE, object, and ground. DiagramBuilder plant_lcs_builder; auto [plant_lcs, scene_graph] = - AddMultibodyPlantSceneGraph(&plant_lcs_builder, 0.0); - std::vector object_indices_lcs = - AddLCSModelsToPlant( - &plant_lcs, &scene_graph, controller_params.object_models, - controller_params.include_end_effector_orientation, - sampling_c3_options.include_walls); + AddMultibodyPlantSceneGraph(&plant_lcs_builder, 0.0); + std::vector object_indices_lcs = AddLCSModelsToPlant( + &plant_lcs, &scene_graph, controller_params.object_models, + controller_params.include_end_effector_orientation, + sampling_c3_options.include_walls); plant_lcs.Finalize(); - std::unique_ptr> plant_lcs_autodiff = drake::systems::System::ToAutoDiffXd(plant_lcs); @@ -138,7 +141,8 @@ int DoMain(int argc, char* argv[]) { std::vector> ee_ground_contact{ SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; - // For each pair of object-object or wall-object, we store the contact pairs between their convex pieces + // For each pair of object-object or wall-object, we store the contact pairs + // between their convex pieces std::vector>> object_object_contact_pairs; std::vector>> wall_object_contact_pairs; @@ -201,8 +205,7 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_1"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])); - } - else if (FLAGS_demo_name == "push_t") { + } else if (FLAGS_demo_name == "push_t") { drake::geometry::GeometryId vertical_geoms = plant_lcs.GetCollisionGeometriesForBody( plant_lcs.GetBodyByName("vertical_link"))[0]; @@ -231,110 +234,117 @@ int DoMain(int argc, char* argv[]) { ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["VERTICAL_LINK"])); - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); - } - else if (FLAGS_demo_name == "anything") { - if (sampling_c3_options.include_walls) { - drake::geometry::GeometryId left_wall_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName("left_wall"))[0]; - drake::geometry::GeometryId right_wall_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName("right_wall"))[0]; - drake::geometry::GeometryId front_wall_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName("front_wall"))[0]; - drake::geometry::GeometryId back_wall_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName("back_wall"))[0]; - - contact_geoms["LEFT_WALL"] = left_wall_geoms; - contact_geoms["RIGHT_WALL"] = right_wall_geoms; - contact_geoms["FRONT_WALL"] = front_wall_geoms; - contact_geoms["BACK_WALL"] = back_wall_geoms; - } - - std::vector> all_object_geoms; - for (int i = 0; i < controller_params.base_names.size(); i++) { // exclude ee/ground - std::string body_name = controller_params.base_names.at(i); - const std::vector& object_geoms = - plant_lcs.GetCollisionGeometriesForBody(plant_lcs.GetBodyByName(body_name)); - - // Each object must contain at least 4 collision geometries: - // 1. The main body: it can be a single mesh or decomposed into multiple convex pieces - // 2. 3 spheres on the top left, top right, and bottom of the object - DRAKE_DEMAND(object_geoms.size() >= 4); - const auto& top_left_sphere_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 3]; - const auto& top_right_sphere_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 2]; - const auto& bottom_sphere_geoms = - plant_lcs.GetCollisionGeometriesForBody( - plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 1]; - contact_geoms["TOP_LEFT_SPHERE_" + std::to_string(i)] = top_left_sphere_geoms; - contact_geoms["TOP_RIGHT_SPHERE_" + std::to_string(i)] = top_right_sphere_geoms; - contact_geoms["BOTTOM_SPHERE_" + std::to_string(i)] = bottom_sphere_geoms; - - const std::vector object_geoms_without_spheres = - std::vector(object_geoms.begin(), object_geoms.end() - 3); - - if (sampling_c3_options.include_walls) { - std::vector wall_geoms{ - contact_geoms["LEFT_WALL"], - contact_geoms["RIGHT_WALL"], - contact_geoms["FRONT_WALL"], - }; - if (sampling_c3_options.include_back_wall) { - wall_geoms.push_back(contact_geoms["BACK_WALL"]); - } - std::vector> convex_piece_pairs; - for (const auto& wall_geom : wall_geoms){ - for (const auto& object_geom : object_geoms_without_spheres){ - convex_piece_pairs.emplace_back(wall_geom, object_geom); - } - } - wall_object_contact_pairs.push_back(std::move(convex_piece_pairs)); - } - - for (int j = 0; j < object_geoms.size() - 3; j++) { - ee_contact_pairs.push_back( - SortedPair(contact_geoms["EE"], object_geoms[j])); - } - all_object_geoms.push_back(object_geoms_without_spheres); - - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["TOP_LEFT_SPHERE_" + std::to_string(i)], contact_geoms["GROUND"])); - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["TOP_RIGHT_SPHERE_" + std::to_string(i)], contact_geoms["GROUND"])); - ground_object_contact_pairs.push_back(SortedPair( - contact_geoms["BOTTOM_SPHERE_" + std::to_string(i)], contact_geoms["GROUND"])); - } - - // Object-object contact pairs (excluding end effector), each pair of - // convex pieces for each pair of objects - for (int i = 0; i + 1 < controller_params.num_objects; i++) { - for (int j = i + 1; j < controller_params.num_objects; j++) { - std::vector> convex_piece_pairs; - const std::vector& object_1_geoms = all_object_geoms.at(i); - const std::vector& object_2_geoms = all_object_geoms.at(j); - - for (const auto& g1 : object_1_geoms) { - for (const auto& g2 : object_2_geoms) { - convex_piece_pairs.emplace_back(g1, g2); - } - } - object_object_contact_pairs.push_back(std::move(convex_piece_pairs)); - } - } - } - else { + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); + } else if (FLAGS_demo_name == "anything") { + if (sampling_c3_options.include_walls) { + drake::geometry::GeometryId left_wall_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("left_wall"))[0]; + drake::geometry::GeometryId right_wall_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("right_wall"))[0]; + drake::geometry::GeometryId front_wall_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("front_wall"))[0]; + drake::geometry::GeometryId back_wall_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("back_wall"))[0]; + + contact_geoms["LEFT_WALL"] = left_wall_geoms; + contact_geoms["RIGHT_WALL"] = right_wall_geoms; + contact_geoms["FRONT_WALL"] = front_wall_geoms; + contact_geoms["BACK_WALL"] = back_wall_geoms; + } + + std::vector> all_object_geoms; + for (int i = 0; i < controller_params.base_names.size(); + i++) { // exclude ee/ground + std::string body_name = controller_params.base_names.at(i); + const std::vector& object_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName(body_name)); + + // Each object must contain at least 4 collision geometries: + // 1. The main body: it can be a single mesh or decomposed into multiple + // convex pieces + // 2. 3 spheres on the top left, top right, and bottom of the object + DRAKE_DEMAND(object_geoms.size() >= 4); + const auto& top_left_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 3]; + const auto& top_right_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 2]; + const auto& bottom_sphere_geoms = plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName(body_name))[object_geoms.size() - 1]; + contact_geoms["TOP_LEFT_SPHERE_" + std::to_string(i)] = + top_left_sphere_geoms; + contact_geoms["TOP_RIGHT_SPHERE_" + std::to_string(i)] = + top_right_sphere_geoms; + contact_geoms["BOTTOM_SPHERE_" + std::to_string(i)] = bottom_sphere_geoms; + + const std::vector + object_geoms_without_spheres = + std::vector(object_geoms.begin(), + object_geoms.end() - 3); + + if (sampling_c3_options.include_walls) { + std::vector wall_geoms{ + contact_geoms["LEFT_WALL"], + contact_geoms["RIGHT_WALL"], + contact_geoms["FRONT_WALL"], + }; + if (sampling_c3_options.include_back_wall) { + wall_geoms.push_back(contact_geoms["BACK_WALL"]); + } + std::vector> convex_piece_pairs; + for (const auto& wall_geom : wall_geoms) { + for (const auto& object_geom : object_geoms_without_spheres) { + convex_piece_pairs.emplace_back(wall_geom, object_geom); + } + } + wall_object_contact_pairs.push_back(std::move(convex_piece_pairs)); + } + + for (int j = 0; j < object_geoms.size() - 3; j++) { + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], object_geoms[j])); + } + all_object_geoms.push_back(object_geoms_without_spheres); + + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["TOP_LEFT_SPHERE_" + std::to_string(i)], + contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["TOP_RIGHT_SPHERE_" + std::to_string(i)], + contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back( + SortedPair(contact_geoms["BOTTOM_SPHERE_" + std::to_string(i)], + contact_geoms["GROUND"])); + } + + // Object-object contact pairs (excluding end effector), each pair of + // convex pieces for each pair of objects + for (int i = 0; i + 1 < controller_params.num_objects; i++) { + for (int j = i + 1; j < controller_params.num_objects; j++) { + std::vector> convex_piece_pairs; + const std::vector& object_1_geoms = all_object_geoms.at(i); + const std::vector& object_2_geoms = all_object_geoms.at(j); + + for (const auto& g1 : object_1_geoms) { + for (const auto& g2 : object_2_geoms) { + convex_piece_pairs.emplace_back(g1, g2); + } + } + object_object_contact_pairs.push_back(std::move(convex_piece_pairs)); + } + } + } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } // Order: EE-ground, EE-object, object-ground, object-object @@ -379,40 +389,38 @@ int DoMain(int argc, char* argv[]) { object_context.get(), kEndEffectorName, controller_params.object_body_name, controller_params.include_end_effector_orientation, - controller_params.base_names - ); + controller_params.base_names); std::cout << "Before target generator" << std::endl; // Select the target generator based on the demo. std::unique_ptr target_generator; if (FLAGS_demo_name == "jacktoy") { - target_generator = - std::make_unique( - plant_object, controller_params.goal_params, object_indices); - } else if (FLAGS_demo_name == "push_t") { target_generator = - std::make_unique( - plant_object, controller_params.goal_params, object_indices); + std::make_unique( + plant_object, controller_params.goal_params, object_indices); + } else if (FLAGS_demo_name == "push_t") { + target_generator = std::make_unique( + plant_object, controller_params.goal_params, object_indices); } else if (FLAGS_demo_name == "anything") { - target_generator = - std::make_unique( - plant_object, controller_params.goal_params, object_indices); - - } else { + target_generator = std::make_unique( + plant_object, controller_params.goal_params, object_indices); + + } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } - auto* control_target = builder.AddSystem(std::move(target_generator)); + auto* control_target = builder.AddSystem(std::move(target_generator)); // Input sizes are EE position (3), object pose (7), EE velocity (3), object // velocities (6). - std::vector input_sizes = {3}; // ee position - for (int i = 0; i < controller_params.num_objects; i++) { // object pose - input_sizes.push_back(7); - } - input_sizes.push_back(3); // ee velocity - for (int i = 0; i < controller_params.num_objects; i++) { // object velocities - input_sizes.push_back(6); - } + std::vector input_sizes = {3}; // ee position + for (int i = 0; i < controller_params.num_objects; i++) { // object pose + input_sizes.push_back(7); + } + input_sizes.push_back(3); // ee velocity + for (int i = 0; i < controller_params.num_objects; + i++) { // object velocities + input_sizes.push_back(6); + } auto target_state_mux = builder.AddSystem(input_sizes); @@ -429,54 +437,59 @@ int DoMain(int argc, char* argv[]) { lcm_channel_params.target_generator_info_channel, &lcm, TriggerTypeSet({TriggerType::kForced}))); - std::cout << "Before muxes" << std::endl; + std::cout << "Before muxes" << std::endl; // Port 0 ee target - builder.Connect(control_target->get_output_port_end_effector_target(), + builder.Connect(control_target->get_output_port_end_effector_target(), target_state_mux->get_input_port(0)); - // Ports 1 to n object targets - std::vector*> output_ports_object_target = - control_target->get_output_ports_object_target(); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(*(output_ports_object_target.at(i)), - target_state_mux->get_input_port(i + 1)); - } - - builder.Connect(end_effector_zero_velocity_source->get_output_port(), // Port n+1 ee velo target - target_state_mux->get_input_port(controller_params.num_objects + 1)); - - // Ports (n+2) to (2n+1) object velo targets - std::vector*> output_ports_object_velocity_target = - control_target->get_output_ports_object_velocity_target(); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(*(output_ports_object_velocity_target.at(i)), - target_state_mux->get_input_port(i + controller_params.num_objects + 2)); - } + // Ports 1 to n object targets + std::vector*> output_ports_object_target = + control_target->get_output_ports_object_target(); + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(*(output_ports_object_target.at(i)), + target_state_mux->get_input_port(i + 1)); + } + + builder.Connect( + end_effector_zero_velocity_source + ->get_output_port(), // Port n+1 ee velo target + target_state_mux->get_input_port(controller_params.num_objects + 1)); + + // Ports (n+2) to (2n+1) object velo targets + std::vector*> output_ports_object_velocity_target = + control_target->get_output_ports_object_velocity_target(); + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(*(output_ports_object_velocity_target.at(i)), + target_state_mux->get_input_port( + i + controller_params.num_objects + 2)); + } builder.Connect(control_target->get_output_port_target_gen_info(), target_gen_info_publisher->get_input_port()); - // Port 0 ee target - builder.Connect(control_target->get_output_port_end_effector_target(), - final_target_state_mux->get_input_port(0)); + // Port 0 ee target + builder.Connect(control_target->get_output_port_end_effector_target(), + final_target_state_mux->get_input_port(0)); - // Ports 1 to n object targets - std::vector*> output_ports_object_final_target = - control_target->get_output_ports_object_final_target(); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(*(output_ports_object_final_target.at(i)), - final_target_state_mux->get_input_port(i + 1)); - } + // Ports 1 to n object targets + std::vector*> output_ports_object_final_target = + control_target->get_output_ports_object_final_target(); + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(*(output_ports_object_final_target.at(i)), + final_target_state_mux->get_input_port(i + 1)); + } - // Port n+1 ee velo target - builder.Connect(end_effector_zero_velocity_source->get_output_port(), - final_target_state_mux->get_input_port(controller_params.num_objects + 1)); + // Port n+1 ee velo target + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + final_target_state_mux->get_input_port( + controller_params.num_objects + 1)); - // Ports (n+2) to (2n+1) constant vector - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(object_zero_velocity_source->get_output_port(), - final_target_state_mux->get_input_port(i + controller_params.num_objects + 2)); - } + // Ports (n+2) to (2n+1) constant vector + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(object_zero_velocity_source->get_output_port(), + final_target_state_mux->get_input_port( + i + controller_params.num_objects + 2)); + } // Sampling C3 controller. auto controller = builder.AddSystem( @@ -501,31 +514,6 @@ int DoMain(int argc, char* argv[]) { lcm_channel_params.c3_object_best_plan_channel, &lcm, TriggerTypeSet({TriggerType::kForced}))); - // C3 senders. - auto c3_output_sender_curr_plan = - builder.AddNamedSystem("c3_output_sender_curr_plan", - std::make_unique()); - auto c3_output_publisher_curr_plan = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_debug_output_curr_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_forces_publisher_curr_plan = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_force_curr_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - - auto c3_output_sender_best_plan = - builder.AddNamedSystem("c3_output_sender_best_plan", - std::make_unique()); - auto c3_output_publisher_best_plan = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_debug_output_best_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - auto c3_forces_publisher_best_plan = - builder.AddSystem(LcmPublisherSystem::Make( - lcm_channel_params.c3_force_best_channel, &lcm, - TriggerTypeSet({TriggerType::kForced}))); - // Systems for publishing the tracking output. auto actor_c3_execution_trajectory_sender = builder.AddSystem( LcmPublisherSystem::Make( @@ -539,7 +527,6 @@ int DoMain(int argc, char* argv[]) { LcmPublisherSystem::Make( lcm_channel_params.tracking_trajectory_actor_channel, &lcm, TriggerTypeSet({TriggerType::kForced}))); - // Sample-related senders/publishers. auto sample_buffer_sender = builder.AddSystem( @@ -594,34 +581,35 @@ int DoMain(int argc, char* argv[]) { lcm_channel_params.dynamically_feasible_best_plan_channel, &lcm, TriggerTypeSet({TriggerType::kForced}))); + std::vector state_names = {"end_effector_x", "end_effector_y", + "end_effector_z"}; - std::vector state_names = { - "end_effector_x", "end_effector_y", "end_effector_z"}; - - std::vector object_pose_names = {"object_qw", "object_qx", "object_qy", "object_qz", "object_x", "object_y", "object_z"}; - std::vector object_velo_names = {"object_wx", "object_wy", "object_wz", "object_vx", "object_vy", "object_vz"}; - for (int i = 0; i < controller_params.num_objects; i++) { - for (int j = 0; j < object_pose_names.size(); j++) { - std::string item = object_pose_names.at(j) + "_" + std::to_string(i); - state_names.push_back(item); - } - } - state_names.push_back("end_effector_vx"); - state_names.push_back("end_effector_vy"); - state_names.push_back("end_effector_vz"); - + std::vector object_pose_names = { + "object_qw", "object_qx", "object_qy", "object_qz", + "object_x", "object_y", "object_z"}; + std::vector object_velo_names = {"object_wx", "object_wy", + "object_wz", "object_vx", + "object_vy", "object_vz"}; for (int i = 0; i < controller_params.num_objects; i++) { - for (int j = 0; j < object_velo_names.size(); j++) { - std::string item = object_velo_names.at(j) + "_" + std::to_string(i); - state_names.push_back(item); - } - } + for (int j = 0; j < object_pose_names.size(); j++) { + std::string item = object_pose_names.at(j) + "_" + std::to_string(i); + state_names.push_back(item); + } + } + state_names.push_back("end_effector_vx"); + state_names.push_back("end_effector_vy"); + state_names.push_back("end_effector_vz"); + for (int i = 0; i < controller_params.num_objects; i++) { + for (int j = 0; j < object_velo_names.size(); j++) { + std::string item = object_velo_names.at(j) + "_" + std::to_string(i); + state_names.push_back(item); + } + } // C3 state senders: actual, target, and final target. auto c3_state_sender = builder.AddSystem( - plant_lcs.num_positions() + plant_lcs.num_velocities(), - state_names); + plant_lcs.num_positions() + plant_lcs.num_velocities(), state_names); auto c3_target_state_publisher = builder.AddSystem(LcmPublisherSystem::Make( lcm_channel_params.c3_target_state_channel, &lcm, @@ -637,27 +625,28 @@ int DoMain(int argc, char* argv[]) { builder.Connect(franka_state_receiver->get_output_port(), reduced_order_model_receiver->get_input_port_franka_state()); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(object_state_subs.at(i)->get_output_port(), - object_state_receivers.at(i)->get_input_port()); - } - - std::vector*> reduced_order_model_receivers = - reduced_order_model_receiver->get_input_ports_object_state(); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(object_state_receivers.at(i)->get_output_port(), - *(reduced_order_model_receivers.at(i))); - } + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(object_state_subs.at(i)->get_output_port(), + object_state_receivers.at(i)->get_input_port()); + } + + std::vector*> + reduced_order_model_receivers = + reduced_order_model_receiver->get_input_ports_object_state(); + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(object_state_receivers.at(i)->get_output_port(), + *(reduced_order_model_receivers.at(i))); + } builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), controller->get_input_port_lcs_state()); - std::vector*> input_ports_object_state = - control_target->get_input_ports_object_state(); - for (int i = 0; i < controller_params.num_objects; i++) { - builder.Connect(object_state_receivers.at(i)->get_output_port(), - *(input_ports_object_state.at(i))); - } + std::vector*> + input_ports_object_state = control_target->get_input_ports_object_state(); + for (int i = 0; i < controller_params.num_objects; i++) { + builder.Connect(object_state_receivers.at(i)->get_output_port(), + *(input_ports_object_state.at(i))); + } builder.Connect(target_state_mux->get_output_port(), controller->get_input_port_target()); @@ -675,34 +664,40 @@ int DoMain(int argc, char* argv[]) { actor_trajectory_sender_best_plan->get_input_port()); builder.Connect(controller->get_output_port_c3_solution_best_plan_object(), object_trajectory_sender_best_plan->get_input_port()); - builder.Connect(controller->get_output_port_c3_solution_curr_plan(), - c3_output_sender_curr_plan->get_input_port_c3_solution()); - builder.Connect(controller->get_output_port_c3_intermediates_curr_plan(), - c3_output_sender_curr_plan->get_input_port_c3_intermediates()); - builder.Connect(controller->get_output_port_lcs_contact_jacobian_curr_plan(), - c3_output_sender_curr_plan->get_input_port_lcs_contact_info()); - builder.Connect(c3_output_sender_curr_plan->get_output_port_c3_debug(), - c3_output_publisher_curr_plan->get_input_port()); - builder.Connect(c3_output_sender_curr_plan->get_output_port_c3_force(), - c3_forces_publisher_curr_plan->get_input_port()); - builder.Connect(controller->get_output_port_c3_solution_best_plan(), - c3_output_sender_best_plan->get_input_port_c3_solution()); - builder.Connect(controller->get_output_port_c3_intermediates_best_plan(), - c3_output_sender_best_plan->get_input_port_c3_intermediates()); - builder.Connect(controller->get_output_port_lcs_contact_jacobian_best_plan(), - c3_output_sender_best_plan->get_input_port_lcs_contact_info()); - builder.Connect(c3_output_sender_best_plan->get_output_port_c3_debug(), - c3_output_publisher_best_plan->get_input_port()); - builder.Connect(c3_output_sender_best_plan->get_output_port_c3_force(), - c3_forces_publisher_best_plan->get_input_port()); - builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_actor(), - dynamically_feasible_curr_plan_actor_publisher->get_input_port()); - builder.Connect(controller->get_output_port_dynamically_feasible_best_plan_actor(), - dynamically_feasible_best_plan_actor_publisher->get_input_port()); - builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_object(), - dynamically_feasible_curr_plan_object_publisher->get_input_port()); - builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_object(), - dynamically_feasible_best_plan_object_publisher->get_input_port()); + + C3OutputGenerator::AddLcmPublisherToBuilder( + builder, controller->get_output_port_c3_solution_curr_plan(), + controller->get_output_port_c3_intermediates_curr_plan(), + lcm_channel_params.c3_debug_output_curr_channel, &lcm, + TriggerTypeSet({TriggerType::kForced})); + ContactForceGenerator::AddLcmPublisherToBuilder( + builder, controller->get_output_port_c3_solution_curr_plan(), + controller->get_output_port_lcs_contact_jacobian_curr_plan(), + lcm_channel_params.c3_force_curr_channel, &lcm, + TriggerTypeSet({TriggerType::kForced})); + C3OutputGenerator::AddLcmPublisherToBuilder( + builder, controller->get_output_port_c3_solution_best_plan(), + controller->get_output_port_c3_intermediates_best_plan(), + lcm_channel_params.c3_debug_output_best_channel, &lcm, + TriggerTypeSet({TriggerType::kForced})); + ContactForceGenerator::AddLcmPublisherToBuilder( + builder, controller->get_output_port_c3_solution_best_plan(), + controller->get_output_port_lcs_contact_jacobian_best_plan(), + lcm_channel_params.c3_force_best_channel, &lcm, + TriggerTypeSet({TriggerType::kForced})); + + builder.Connect( + controller->get_output_port_dynamically_feasible_curr_plan_actor(), + dynamically_feasible_curr_plan_actor_publisher->get_input_port()); + builder.Connect( + controller->get_output_port_dynamically_feasible_best_plan_actor(), + dynamically_feasible_best_plan_actor_publisher->get_input_port()); + builder.Connect( + controller->get_output_port_dynamically_feasible_curr_plan_object(), + dynamically_feasible_curr_plan_object_publisher->get_input_port()); + builder.Connect( + controller->get_output_port_dynamically_feasible_curr_plan_object(), + dynamically_feasible_best_plan_object_publisher->get_input_port()); builder.Connect(target_state_mux->get_output_port(), c3_state_sender->get_input_port_target_state()); builder.Connect(final_target_state_mux->get_output_port(), @@ -735,12 +730,15 @@ int DoMain(int argc, char* argv[]) { sample_buffer_sender->get_input_port_samples()); builder.Connect(controller->get_output_port_sample_buffer_costs(), sample_buffer_sender->get_input_port_sample_costs()); - builder.Connect(unsuccessful_sample_buffer_sender->get_output_port_sample_buffer(), - unsuccessful_sample_buffer_publisher->get_input_port()); - builder.Connect(controller->get_output_port_unsuccessful_sample_buffer_configurations(), - unsuccessful_sample_buffer_sender->get_input_port_samples()); - builder.Connect(controller->get_output_port_unsuccessful_sample_buffer_costs(), - unsuccessful_sample_buffer_sender->get_input_port_sample_costs()); + builder.Connect( + unsuccessful_sample_buffer_sender->get_output_port_sample_buffer(), + unsuccessful_sample_buffer_publisher->get_input_port()); + builder.Connect( + controller->get_output_port_unsuccessful_sample_buffer_configurations(), + unsuccessful_sample_buffer_sender->get_input_port_samples()); + builder.Connect( + controller->get_output_port_unsuccessful_sample_buffer_costs(), + unsuccessful_sample_buffer_sender->get_input_port_sample_costs()); std::cout << "Before drawandsave" << std::endl; auto owned_diagram = builder.Build(); @@ -749,7 +747,7 @@ int DoMain(int argc, char* argv[]) { DrawAndSaveDiagramGraph(*owned_diagram); DrawAndSaveDiagramGraph(*plant_lcs_diagram); - // Run lcm-driven simulation. The buffer size argument is needed to ensure + // Run lcm-driven simulation. The buffer size argument is needed to ensure // the latest messages are used in the control loop. See // https://github.com/DAIRLab/dairlib/pull/366 for more details. int lcm_buffer_size = 200; @@ -757,27 +755,23 @@ int DoMain(int argc, char* argv[]) { systems::LcmDrivenLoop loop( &lcm, shared_diagram, franka_state_receiver, lcm_channel_params.franka_state_channel, true, lcm_buffer_size); - std::cout << "constructed loop" << std::endl; - - LcmHandleSubscriptionsUntil( - &lcm, - [&]() { - int total_count = 0; - for (const auto& sub : object_state_subs) { - if (sub->GetInternalMessageCount() <= 1) { - return false; - } - } - return true; - }); + std::cout << "constructed loop" << std::endl; + + LcmHandleSubscriptionsUntil(&lcm, [&]() { + int total_count = 0; + for (const auto& sub : object_state_subs) { + if (sub->GetInternalMessageCount() <= 1) { + return false; + } + } + return true; + }); std::cout << "After LcmHandleSubscriptionsUntil" << std::endl; - //std::this_thread::sleep_for(std::chrono::seconds(20)); + // std::this_thread::sleep_for(std::chrono::seconds(20)); loop.Simulate(); return 0; } } // namespace dairlib - - int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_visualizer.cc b/examples/sampling_c3/franka_visualizer.cc index d9411e9578..3c248acf06 100644 --- a/examples/sampling_c3/franka_visualizer.cc +++ b/examples/sampling_c3/franka_visualizer.cc @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -7,13 +7,13 @@ #include "common/eigen_utils.h" #include "common/find_resource.h" #include "dairlib/lcmt_robot_output.hpp" -#include "examples/sampling_c3/sampling_c3_utils.h" #include "examples/sampling_c3/c3_mode_visualizer.h" -#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" #include "examples/sampling_c3/parameter_headers/lcm_channels.h" -#include "examples/sampling_c3/parameter_headers/visualizer_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" #include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" #include "examples/sampling_c3/parameter_headers/sampling_params.h" +#include "examples/sampling_c3/parameter_headers/visualizer_params.h" +#include "examples/sampling_c3/sampling_c3_utils.h" #include "multibody/com_pose_system.h" #include "multibody/multibody_utils.h" #include "multibody/visualization_utils.h" @@ -67,7 +67,6 @@ using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::Parser; using drake::systems::DiagramBuilder; - DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); DEFINE_string(demo_name, "jacktoy", "Name for the demo, used when building filepaths for output."); @@ -76,8 +75,9 @@ int do_main(int argc, char* argv[]) { gflags::ParseCommandLineFlags(&argc, &argv, true); // Load parameters. - std::string controller_params_path = "examples/sampling_c3/" + - FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + std::string controller_params_path = + "examples/sampling_c3/" + FLAGS_demo_name + + "/parameters/sampling_c3_controller_params.yaml"; SamplingC3ControllerParams controller_params = drake::yaml::LoadYamlFile( controller_params_path); @@ -86,9 +86,9 @@ int do_main(int argc, char* argv[]) { controller_params.vis_params_file); SamplingC3Options sampling_c3_options = controller_params.sampling_c3_options; SamplingParams sampling_params = controller_params.sampling_params; - std::string lcm_channels_file = FLAGS_is_simulation ? - controller_params.lcm_channels_simulation_file : - controller_params.lcm_channels_hardware_file; + std::string lcm_channels_file = + FLAGS_is_simulation ? controller_params.lcm_channels_simulation_file + : controller_params.lcm_channels_hardware_file; SamplingC3LcmChannels lcm_channel_params = drake::yaml::LoadYamlFile(lcm_channels_file); @@ -100,46 +100,48 @@ int do_main(int argc, char* argv[]) { // Build the visualizer plant. MultibodyPlant plant(0.0); ModelInstanceIndex franka_index = AddFrankaToPlant( - &plant, &scene_graph, true, true, sampling_c3_options.include_walls); + &plant, &scene_graph, true, true, sampling_c3_options.include_walls); - // Getting vector of object indices for all objects - std::vector object_indices_plant = AddObjectsToPlant( - &plant, &scene_graph, vis_params.object_vis_models); + // Getting vector of object indices for all objects + std::vector object_indices_plant = + AddObjectsToPlant(&plant, &scene_graph, vis_params.object_vis_models); plant.Finalize(); - // Create a Franka-only plant. MultibodyPlant plant_franka(0.0); - ModelInstanceIndex franka_index0 = AddFrankaToPlant( - &plant_franka, nullptr, true, false); + ModelInstanceIndex franka_index0 = + AddFrankaToPlant(&plant_franka, nullptr, true, false); plant_franka.Finalize(); auto franka_context = plant_franka.CreateDefaultContext(); - std::cout << "Franka index plant: " << franka_index << std::endl; - std::cout << "Franka index plant_franka: " << franka_index0 << std::endl; + std::cout << "Franka index plant: " << franka_index << std::endl; + std::cout << "Franka index plant_franka: " << franka_index0 << std::endl; // Create an object-only plant. MultibodyPlant plant_object(0.0); - std::vector object_indices = AddObjectsToPlant(&plant_object, nullptr, - vis_params.object_vis_models); + std::vector object_indices = + AddObjectsToPlant(&plant_object, nullptr, vis_params.object_vis_models); plant_object.Finalize(); auto object_context = plant_object.CreateDefaultContext(); - std::cout << std::endl; + std::cout << std::endl; - std::cout << "Object_plant num_pos: " << plant_object.num_positions() << std::endl; - std::cout << "Object_plant num_velo: " << plant_object.num_velocities() << std::endl; + std::cout << "Object_plant num_pos: " << plant_object.num_positions() + << std::endl; + std::cout << "Object_plant num_velo: " << plant_object.num_velocities() + << std::endl; - //std::this_thread::sleep_for(std::chrono::seconds(15)); + // std::this_thread::sleep_for(std::chrono::seconds(15)); auto lcm = builder.AddSystem(); auto franka_state_receiver = builder.AddSystem(plant, franka_index); - - // Duplicating object state reciever for each object + + // Duplicating object state reciever for each object std::vector object_state_receivers; - for (ModelInstanceIndex obj_index : object_indices_plant) { - object_state_receivers.push_back(builder.AddSystem(plant, obj_index)); + for (ModelInstanceIndex obj_index : object_indices_plant) { + object_state_receivers.push_back( + builder.AddSystem(plant, obj_index)); } auto franka_passthrough = builder.AddSystem( @@ -151,53 +153,48 @@ int do_main(int argc, char* argv[]) { // Duplicating passthrough for each object std::vector*> tray_passthroughs; - for (int i = 0; i < object_state_receivers.size(); i++) { - tray_passthroughs.push_back( - builder.AddSystem( - object_state_receivers.at(i)->get_output_port(0).size(), 0, - plant.num_positions(object_indices_plant.at(i))) - ); + for (int i = 0; i < object_state_receivers.size(); i++) { + tray_passthroughs.push_back(builder.AddSystem( + object_state_receivers.at(i)->get_output_port(0).size(), 0, + plant.num_positions(object_indices_plant.at(i)))); } std::vector input_sizes = {plant.num_positions(franka_index)}; - for (ModelInstanceIndex obj_index : object_indices_plant) { - input_sizes.push_back( - plant.num_positions(obj_index) - ); - } - + for (ModelInstanceIndex obj_index : object_indices_plant) { + input_sizes.push_back(plant.num_positions(obj_index)); + } + auto mux = builder.AddSystem>(input_sizes); auto reduced_order_model_receiver = builder.AddSystem( plant_franka, franka_context.get(), plant_object, object_context.get(), kEndEffectorName, - controller_params.object_body_name, false, controller_params.base_names); + controller_params.object_body_name, false, + controller_params.base_names); builder.Connect(franka_state_receiver->get_output_port(), reduced_order_model_receiver->get_input_port_franka_state()); - std::vector*> ro_model_object_inputs = + std::vector*> ro_model_object_inputs = reduced_order_model_receiver->get_input_ports_object_state(); for (int i = 0; i < object_state_receivers.size(); i++) { builder.Connect(object_state_receivers.at(i)->get_output_port(), *(ro_model_object_inputs.at(i))); - } - std::cout << "After loop?" << std::endl; + } + std::cout << "After loop?" << std::endl; // LCM subscribers. auto franka_state_sub = builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.franka_state_channel, lcm)); - std::vector object_state_subs; - for (int i = 0; i < object_state_receivers.size(); i++) { - object_state_subs.push_back( - builder.AddSystem(LcmSubscriberSystem::Make( - lcm_channel_params.object_state_channels.at(i), lcm)) - ); - } + for (int i = 0; i < object_state_receivers.size(); i++) { + object_state_subs.push_back( + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.object_state_channels.at(i), lcm))); + } auto is_c3_mode_sub = builder.AddSystem( LcmSubscriberSystem::Make( @@ -216,8 +213,8 @@ int do_main(int argc, char* argv[]) { auto trajectory_sub_object_curr = builder.AddSystem( LcmSubscriberSystem::Make( lcm_channel_params.c3_object_curr_plan_channel, lcm)); - auto trajectory_sub_force_curr = builder.AddSystem( - LcmSubscriberSystem::Make( + auto trajectory_sub_force_curr = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.c3_force_curr_channel, lcm)); auto dynamically_feasible_trajectory_sub_object_curr = builder.AddSystem( LcmSubscriberSystem::Make( @@ -233,8 +230,8 @@ int do_main(int argc, char* argv[]) { auto trajectory_sub_object_best = builder.AddSystem( LcmSubscriberSystem::Make( lcm_channel_params.c3_object_best_plan_channel, lcm)); - auto trajectory_sub_force_best = builder.AddSystem( - LcmSubscriberSystem::Make( + auto trajectory_sub_force_best = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.c3_force_best_channel, lcm)); auto dynamically_feasible_trajectory_sub_object_best = builder.AddSystem( LcmSubscriberSystem::Make( @@ -247,21 +244,21 @@ int do_main(int argc, char* argv[]) { auto sample_location_sub = builder.AddSystem( LcmSubscriberSystem::Make( lcm_channel_params.sample_locations_channel, lcm)); - auto sample_buffer_sub = builder.AddSystem( - LcmSubscriberSystem::Make( + auto sample_buffer_sub = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.sample_buffer_channel, lcm)); auto sample_costs_sub = builder.AddSystem( LcmSubscriberSystem::Make( lcm_channel_params.sample_costs_channel, lcm)); - auto c3_state_actual_sub = builder.AddSystem( - LcmSubscriberSystem::Make( + auto c3_state_actual_sub = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.c3_actual_state_channel, lcm)); - auto c3_state_target_sub = builder.AddSystem( - LcmSubscriberSystem::Make( + auto c3_state_target_sub = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.c3_target_state_channel, lcm)); - auto c3_final_state_target_sub = builder.AddSystem( - LcmSubscriberSystem::Make( + auto c3_final_state_target_sub = + builder.AddSystem(LcmSubscriberSystem::Make( lcm_channel_params.c3_final_target_state_channel, lcm)); auto to_pose = @@ -275,9 +272,9 @@ int do_main(int argc, char* argv[]) { if (vis_params.visualize_c3_workspace) { // Note: There are also robot radius limits which are not visualized. double width = sampling_c3_options.workspace_limits[0][4] - - sampling_c3_options.workspace_limits[0][3]; // x + sampling_c3_options.workspace_limits[0][3]; // x double depth = sampling_c3_options.workspace_limits[1][4] - - sampling_c3_options.workspace_limits[1][3]; // y + sampling_c3_options.workspace_limits[1][3]; // y double height = sampling_c3_options.workspace_limits[2][4] - sampling_c3_options.workspace_limits[2][3]; // z Vector3d workspace_center = { @@ -358,24 +355,22 @@ int do_main(int argc, char* argv[]) { std::cout << "SIZE: " << object_indices.size() << std::endl; for (int i = 0; i < object_indices.size(); i++) { position_names.push_back("object_position_target_" + std::to_string(i)); - orientation_names.push_back("object_orientation_target_" + std::to_string(i)); + orientation_names.push_back("object_orientation_target_" + + std::to_string(i)); } if (vis_params.visualize_c3_plan_curr) { - auto object_pose_drawer_curr = - builder.AddSystem( - meshcat, vis_params.object_vis_models, - position_names, orientation_names, - "plans/curr_planned", sampling_c3_options.N, true, - vis_params.c3_curr_object_color); + auto object_pose_drawer_curr = builder.AddSystem( + meshcat, vis_params.object_vis_models, position_names, + orientation_names, "plans/curr_planned", sampling_c3_options.N, true, + vis_params.c3_curr_object_color); builder.Connect(trajectory_sub_object_curr->get_output_port(), object_pose_drawer_curr->get_input_port_trajectory()); auto end_effector_pose_drawer_curr = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.ee_vis_model), + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), "end_effector_position_target", "end_effector_orientation_target", "plans/curr_planned", sampling_c3_options.N, false, vis_params.c3_curr_ee_color); @@ -384,9 +379,8 @@ int do_main(int argc, char* argv[]) { auto dynamically_feasible_object_pose_drawer_curr = builder.AddSystem( - meshcat, vis_params.object_vis_models, - position_names, orientation_names, - "plans/dynamically_feasible_curr_plan", + meshcat, vis_params.object_vis_models, position_names, + orientation_names, "plans/dynamically_feasible_curr_plan", sampling_c3_options.N + 1, true, vis_params.df_curr_object_color); builder.Connect( dynamically_feasible_trajectory_sub_object_curr->get_output_port(), @@ -395,11 +389,10 @@ int do_main(int argc, char* argv[]) { auto dynamically_feasible_actor_pose_drawer_curr_actor = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.ee_vis_model), + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), "ee_position_target", "end_effector_orientation_target", - "plans/dynamically_feasible_curr_plan", - sampling_c3_options.N + 1, false, vis_params.df_curr_ee_color); + "plans/dynamically_feasible_curr_plan", sampling_c3_options.N + 1, + false, vis_params.df_curr_ee_color); builder.Connect( dynamically_feasible_trajectory_sub_actor_curr->get_output_port(), dynamically_feasible_actor_pose_drawer_curr_actor @@ -408,8 +401,7 @@ int do_main(int argc, char* argv[]) { if (vis_params.visualize_c3_plan_best) { auto object_pose_drawer_best = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.object_vis_model), + meshcat, FindResourceOrThrow(vis_params.object_vis_model), "object_position_target", "object_orientation_target", "plans/best_planned", sampling_c3_options.N, true, vis_params.c3_best_object_color); @@ -418,8 +410,7 @@ int do_main(int argc, char* argv[]) { auto end_effector_pose_drawer_best = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.ee_vis_model), + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), "end_effector_position_target", "end_effector_orientation_target", "plans/best_planned", sampling_c3_options.N, false, vis_params.c3_best_ee_color); @@ -428,8 +419,7 @@ int do_main(int argc, char* argv[]) { auto dynamically_feasible_object_pose_drawer_best = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.object_vis_model), + meshcat, FindResourceOrThrow(vis_params.object_vis_model), "object_position_target", "object_orientation_target", "plans/dynamically_feasible_best_plan", sampling_c3_options.N + 1, true, vis_params.df_best_object_color); @@ -440,11 +430,10 @@ int do_main(int argc, char* argv[]) { auto dynamically_feasible_actor_pose_drawer_best_actor = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.ee_vis_model), + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), "ee_position_target", "end_effector_orientation_target", - "plans/dynamically_feasible_best_plan", - sampling_c3_options.N + 1, false, vis_params.df_best_ee_color); + "plans/dynamically_feasible_best_plan", sampling_c3_options.N + 1, + false, vis_params.df_best_ee_color); builder.Connect( dynamically_feasible_trajectory_sub_actor_best->get_output_port(), dynamically_feasible_actor_pose_drawer_best_actor @@ -457,8 +446,7 @@ int do_main(int argc, char* argv[]) { from_buffer = 1; } auto sample_locations_drawer = builder.AddSystem( - meshcat, - FindResourceOrThrow(vis_params.ee_vis_model), + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), "sample_locations", "unused_orientation_name", "samples", std::max(sampling_params.num_additional_samples_c3 + from_buffer, sampling_params.num_additional_samples_repos + 1) + @@ -489,8 +477,8 @@ int do_main(int argc, char* argv[]) { } if (vis_params.visualize_c3_state) { - auto c3_target_drawer = - builder.AddSystem(meshcat, vis_params.object_vis_models.size(), true, true); + auto c3_target_drawer = builder.AddSystem( + meshcat, vis_params.object_vis_models.size(), true, true); builder.Connect(c3_state_actual_sub->get_output_port(), c3_target_drawer->get_input_port_c3_state_actual()); builder.Connect(c3_state_target_sub->get_output_port(), @@ -532,7 +520,8 @@ int do_main(int argc, char* argv[]) { } if (vis_params.visualize_is_c3_mode) { - auto c3_mode_visualizer = builder.AddSystem(plant_object); + auto c3_mode_visualizer = + builder.AddSystem(plant_object); builder.Connect(is_c3_mode_sub->get_output_port(), c3_mode_visualizer->get_input_port_is_c3_mode()); builder.Connect(reduced_order_model_receiver->get_output_port(), @@ -546,12 +535,12 @@ int do_main(int argc, char* argv[]) { is_c3_mode_drawer->get_input_port_trajectory()); } - builder.Connect(franka_passthrough->get_output_port(), mux->get_input_port(0)); - for (int i = 1; i <= tray_passthroughs.size(); i++) { - builder.Connect(tray_passthroughs.at(i-1)->get_output_port(), mux->get_input_port(i)); - } + for (int i = 1; i <= tray_passthroughs.size(); i++) { + builder.Connect(tray_passthroughs.at(i - 1)->get_output_port(), + mux->get_input_port(i)); + } builder.Connect(*mux, *to_pose); builder.Connect( to_pose->get_output_port(), @@ -559,14 +548,16 @@ int do_main(int argc, char* argv[]) { builder.Connect(*franka_state_receiver, *franka_passthrough); builder.Connect(*franka_state_receiver, *robot_time_passthrough); - for (int i = 0; i < object_state_receivers.size(); i++) { - builder.Connect(*(object_state_receivers.at(i)), *(tray_passthroughs.at(i))); - } + for (int i = 0; i < object_state_receivers.size(); i++) { + builder.Connect(*(object_state_receivers.at(i)), + *(tray_passthroughs.at(i))); + } builder.Connect(*franka_state_sub, *franka_state_receiver); - for (int i = 0; i < object_state_subs.size(); i++) { - builder.Connect(*(object_state_subs.at(i)), *(object_state_receivers.at(i))); - } + for (int i = 0; i < object_state_subs.size(); i++) { + builder.Connect(*(object_state_subs.at(i)), + *(object_state_receivers.at(i))); + } auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( &builder, scene_graph, meshcat, std::move(params)); @@ -579,20 +570,18 @@ int do_main(int argc, char* argv[]) { auto& franka_state_sub_context = diagram->GetMutableSubsystemContext(*franka_state_sub, context.get()); - std::vector*> object_state_sub_contexts; - for (int i = 0; i < object_state_receivers.size(); i++) { - object_state_sub_contexts.push_back( - &diagram->GetMutableSubsystemContext(*(object_state_subs.at(i)), context.get()) - ); - } + std::vector*> object_state_sub_contexts; + for (int i = 0; i < object_state_receivers.size(); i++) { + object_state_sub_contexts.push_back(&diagram->GetMutableSubsystemContext( + *(object_state_subs.at(i)), context.get())); + } franka_state_receiver->InitializeSubscriberPositions( plant, franka_state_sub_context); - for (int i = 0; i < object_state_receivers.size(); i++) { - object_state_receivers.at(i)->InitializeSubscriberPositions( - plant, *object_state_sub_contexts.at(i)); - } - + for (int i = 0; i < object_state_receivers.size(); i++) { + object_state_receivers.at(i)->InitializeSubscriberPositions( + plant, *object_state_sub_contexts.at(i)); + } /// Use the simulator to drive at a fixed rate /// If set_publish_every_time_step is true, this publishes twice @@ -613,6 +602,4 @@ int do_main(int argc, char* argv[]) { } // namespace dairlib -int main(int argc, char* argv[]) { - return dairlib::do_main(argc, argv); -} +int main(int argc, char* argv[]) { return dairlib::do_main(argc, argv); } diff --git a/examples/sampling_c3/jacktoy/BUILD.bazel b/examples/sampling_c3/jacktoy/BUILD.bazel index faec88ea69..3f469f7ded 100644 --- a/examples/sampling_c3/jacktoy/BUILD.bazel +++ b/examples/sampling_c3/jacktoy/BUILD.bazel @@ -44,7 +44,7 @@ cc_library( "//examples/sampling_c3:shared_params_yamls", ], deps = [ - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) @@ -57,7 +57,7 @@ cc_library( ], deps = [ "//examples/sampling_c3/parameter_headers:goal_params", - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) @@ -70,7 +70,7 @@ cc_library( ], deps = [ "//common/parameters:franka_drake_lcm_driver_channels", - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) diff --git a/examples/sampling_c3/parameter_headers/sampling_c3_options.h b/examples/sampling_c3/parameter_headers/sampling_c3_options.h index 21efd9cd7e..c763a0a3a3 100644 --- a/examples/sampling_c3/parameter_headers/sampling_c3_options.h +++ b/examples/sampling_c3/parameter_headers/sampling_c3_options.h @@ -39,7 +39,6 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { int num_planar_contacts; int n_lambda_with_tangential; - std::vector num_friction_directions_per_contact; std::vector starting_index_per_contact_in_lambda_t_vector; int num_planar_contacts_cost; @@ -124,7 +123,8 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { std::vector workspace_limits; ///< Workspace boundaries as vectors. double workspace_margins; ///< Margins to be maintained within the workspace. - std::vector ee_velocity_limits; ///< Limits for end-effector velocities. + std::vector + ee_velocity_limits; ///< Limits for end-effector velocities. C3Options c3_options_pose; LCSFactoryOptions lcs_factory_options_pose; @@ -243,10 +243,11 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { num_contacts_for_cost = std::accumulate(resolve_contacts_to_for_cost.begin(), resolve_contacts_to_for_cost.end(), 0); - mu = std::vector(); + mu_per_contact = std::vector(); for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { int repeat = resolve_contacts_to_lists[num_contacts_index][i]; - mu.value().insert(mu.value().end(), repeat, mu_per_pair_type[i]); + mu_per_contact.value().insert(mu_per_contact.value().end(), repeat, + mu_per_pair_type[i]); } mu_for_cost = std::vector(); for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { @@ -265,28 +266,32 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { resolve_contacts_to_for_cost, num_friction_directions.value()); - for (size_t i = 0; i < num_contacts; ++i) { + for (size_t i = 0; i < static_cast(num_contacts.value()); ++i) { starting_index_per_contact_in_lambda_t_vector.push_back( - 2 * std::accumulate(num_friction_directions_per_contact.begin(), - num_friction_directions_per_contact.begin() + i, - 0)); + 2 * std::accumulate( + num_friction_directions_per_contact.value().begin(), + num_friction_directions_per_contact.value().begin() + i, 0)); } - for (size_t i = 0; i < num_contacts_for_cost; ++i) { + for (size_t i = 0; i < static_cast(num_contacts_for_cost); ++i) { starting_index_per_contact_in_lambda_t_vector_cost.push_back( 2 * std::accumulate( num_friction_directions_per_contact_for_cost.begin(), num_friction_directions_per_contact_for_cost.begin() + i, 0)); } - n_lambda_with_tangential = 2 * num_friction_directions.value() * - (num_contacts - num_planar_contacts) + - 2 * num_planar_contacts; + n_lambda_with_tangential = + 2 * num_friction_directions.value() * + (num_contacts.value() - num_planar_contacts) + + 2 * num_planar_contacts; n_lambda_with_tangential_cost = 2 * num_friction_directions.value() * (num_contacts_for_cost - num_planar_contacts_cost) + 2 * num_planar_contacts_cost; // Create C3 options for both pose and position tracking. + std::cout + << "Setting C3 and LCS factory options for pose and position tracking." + << std::endl; SetCommonOptions(&c3_options_pose, &lcs_factory_options_pose); SetPoseTrackingOptions(&c3_options_pose, &lcs_factory_options_pose); SetCommonOptions(&c3_options_position, &lcs_factory_options_position); @@ -413,7 +418,7 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { lcs_factory_options->num_contacts = num_contacts; lcs_factory_options->num_friction_directions = num_friction_directions; lcs_factory_options->spring_stiffness = 0; // not used in sampling C3 - lcs_factory_options->mu = mu; + lcs_factory_options->mu_per_contact = mu_per_contact; lcs_factory_options->N = N; lcs_factory_options->num_friction_directions_per_contact = num_friction_directions_per_contact; @@ -568,7 +573,7 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions { int num_planar_contacts = 0; int planar_contact = 1; std::vector num_friction_directions_per_contact; - for (int i = 0; i < resolve_contacts_to_list.size(); ++i) { + for (size_t i = 0; i < resolve_contacts_to_list.size(); ++i) { for (int j = 0; j < resolve_contacts_to_list[i]; ++j) { num_planar_contacts += (resolve_as_planar_contacts_list[i] ? 1 : 0); num_friction_directions_per_contact.push_back( diff --git a/examples/sampling_c3/push_t/BUILD.bazel b/examples/sampling_c3/push_t/BUILD.bazel index bd09516749..b1a2ddeb3e 100644 --- a/examples/sampling_c3/push_t/BUILD.bazel +++ b/examples/sampling_c3/push_t/BUILD.bazel @@ -44,7 +44,7 @@ cc_library( "//examples/sampling_c3:shared_params_yamls", ], deps = [ - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) @@ -57,7 +57,7 @@ cc_library( ], deps = [ "//examples/sampling_c3/parameter_headers:goal_params", - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) @@ -70,7 +70,7 @@ cc_library( ], deps = [ "//common/parameters:franka_drake_lcm_driver_channels", - "//solvers:c3", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) diff --git a/examples/sampling_c3/test/BUILD.bazel b/examples/sampling_c3/test/BUILD.bazel index dfb92c7d50..dd70efa382 100644 --- a/examples/sampling_c3/test/BUILD.bazel +++ b/examples/sampling_c3/test/BUILD.bazel @@ -12,9 +12,6 @@ cc_binary( "//multibody:utils", "//systems:system_utils", "//common:math_utils", - "//solvers:c3", - "//solvers:lcs", - "//solvers:c3_output", "//solvers:solver_options_io", "//systems/framework:vector", "@drake//:drake_shared_library", @@ -22,5 +19,6 @@ cc_binary( "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", "//examples/sampling_c3/parameter_headers:franka_sim_params", "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "@c3//:libc3", ], ) diff --git a/examples/sampling_c3/test/lcm_log_loader.cc b/examples/sampling_c3/test/lcm_log_loader.cc index 339fbeeb25..0fd8e03f2c 100644 --- a/examples/sampling_c3/test/lcm_log_loader.cc +++ b/examples/sampling_c3/test/lcm_log_loader.cc @@ -12,6 +12,9 @@ #include #include +#include "c3/core/c3_options.h" +#include "c3/multibody/lcs_factory_options.h" +#include "c3/lcmt_output.hpp" #include "dairlib/lcmt_c3_state.hpp" #include "dairlib/lcmt_object_state.hpp" #include "dairlib/lcmt_radio_out.hpp" @@ -21,8 +24,6 @@ #include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" #include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" #include "examples/sampling_c3/sampling_c3_utils.h" -#include "solvers/c3_output.h" -#include "solvers/lcs_factory.h" #include "systems/controllers/sampling_based_c3_controller.h" #include "systems/framework/timestamped_vector.h" #include "systems/system_utils.h" @@ -58,6 +59,8 @@ DEFINE_string(demo_name, "unkown", namespace dairlib { +using c3::C3Options; +using c3::multibody::LCSFactory; using dairlib::systems::TimestampedVector; using drake::SortedPair; using drake::geometry::GeometryId; @@ -66,7 +69,6 @@ using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; using drake::systems::DiagramBuilder; -using solvers::LCSFactory; // Declare function that will generate samples around T location. std::vector GenerateEvenlySpacedSamplesAroundT( @@ -243,16 +245,12 @@ int DoMain(int argc, char* argv[]) { Eigen::VectorXd x_lcs_actual = Eigen::VectorXd::Zero(19); Eigen::VectorXd x_lcs_desired = Eigen::VectorXd::Zero(19); Eigen::VectorXd x_lcs_final_desired = Eigen::VectorXd::Zero(19); - Eigen::MatrixXd dyn_feas_curr_plan_obj_pos = - Eigen::MatrixXd::Zero(3, N + 1); - Eigen::MatrixXd dyn_feas_curr_plan_ee_pos = - Eigen::MatrixXd::Zero(3, N + 1); + Eigen::MatrixXd dyn_feas_curr_plan_obj_pos = Eigen::MatrixXd::Zero(3, N + 1); + Eigen::MatrixXd dyn_feas_curr_plan_ee_pos = Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_curr_plan_obj_orientation = Eigen::MatrixXd::Zero(4, N + 1); - Eigen::MatrixXd dyn_feas_best_plan_obj_pos = - Eigen::MatrixXd::Zero(3, N + 1); - Eigen::MatrixXd dyn_feas_best_plan_ee_pos = - Eigen::MatrixXd::Zero(3, N + 1); + Eigen::MatrixXd dyn_feas_best_plan_obj_pos = Eigen::MatrixXd::Zero(3, N + 1); + Eigen::MatrixXd dyn_feas_best_plan_ee_pos = Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_best_plan_obj_orientation = Eigen::MatrixXd::Zero(4, N + 1); @@ -405,7 +403,7 @@ int DoMain(int argc, char* argv[]) { } } else if (event->channel == "C3_DEBUG_CURR") { if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { - dairlib::lcmt_c3_output message; + c3::lcmt_output message; if (message.decode(event->data, 0, event->datalen) > 0) { std::cout << "Received C3_DEBUG_CURR message in seconds utime: " << (message.utime) / 1e6 << " and event timestamp " @@ -413,31 +411,31 @@ int DoMain(int argc, char* argv[]) { for (int i = 0; i < 3; i++) { for (int j = 0; j < N; j++) { u_sol(i, j) = - static_cast(message.c3_solution.u_sol[i][j]); + static_cast(message.solution.u_sol[i][j]); } } for (int i = 0; i < 19; i++) { for (int j = 0; j < N; j++) { x_sol(i, j) = - static_cast(message.c3_solution.x_sol[i][j]); + static_cast(message.solution.x_sol[i][j]); } } for (int i = 0; i < 16; i++) { for (int j = 0; j < N; j++) { lambda_sol(i, j) = - static_cast(message.c3_solution.lambda_sol[i][j]); + static_cast(message.solution.lambda_sol[i][j]); } } for (int i = 0; i < 38; i++) { for (int j = 0; j < N; j++) { w_sol(i, j) = - static_cast(message.c3_intermediates.w_sol[i][j]); + static_cast(message.intermediates.w_sol[i][j]); } } for (int i = 0; i < 38; i++) { for (int j = 0; j < N; j++) { delta_sol(i, j) = - static_cast(message.c3_intermediates.delta_sol[i][j]); + static_cast(message.intermediates.delta_sol[i][j]); } } } else { @@ -466,7 +464,7 @@ int DoMain(int argc, char* argv[]) { << "seconds utime: " << (message.utime) / 1e6 << " and event " << "timestamp " << adjusted_utimestamp / 1e6 << std::endl; - for (int i = 0; + for (size_t i = 0; i < message.saved_traj.trajectories[0].datapoints[0].size(); i++) { Eigen::VectorXd sample_location = Eigen::VectorXd::Zero(3); @@ -488,7 +486,7 @@ int DoMain(int argc, char* argv[]) { << "seconds utime: " << (message.utime) / 1e6 << " and event " << "timestamp " << adjusted_utimestamp / 1e6 << std::endl; - for (int i = 0; + for (size_t i = 0; i < message.saved_traj.trajectories[0].datapoints[0].size(); i++) { sample_costs_in_log.push_back(Eigen::VectorXd::Constant( @@ -505,14 +503,12 @@ int DoMain(int argc, char* argv[]) { if ((x_lcs_actual != Eigen::VectorXd::Zero(19)) && (x_lcs_desired != Eigen::VectorXd::Zero(19)) && (x_lcs_final_desired != Eigen::VectorXd::Zero(19)) && - (dyn_feas_curr_plan_ee_pos != - Eigen::MatrixXd::Zero(3, N + 1)) && - (dyn_feas_curr_plan_obj_pos != - Eigen::MatrixXd::Zero(3, N + 1)) && + (dyn_feas_curr_plan_ee_pos != Eigen::MatrixXd::Zero(3, N + 1)) && + (dyn_feas_curr_plan_obj_pos != Eigen::MatrixXd::Zero(3, N + 1)) && (dyn_feas_curr_plan_obj_orientation != Eigen::MatrixXd::Zero(4, N + 1)) && - (u_sol != Eigen::MatrixXd::Zero(3, N)) && - (is_c3_mode_set) && (sample_locations_in_log.size() > 0) && + (u_sol != Eigen::MatrixXd::Zero(3, N)) && (is_c3_mode_set) && + (sample_locations_in_log.size() > 0) && (sample_costs_in_log.size() > 0)) { break; } @@ -1015,13 +1011,13 @@ int DoMain(int argc, char* argv[]) { std::cout << "Finished ForcedPublish" << std::endl; // Example of getting the C3 solution by evaluating the output port. - // auto c3_solution = controller->get_output_port_c3_solution_curr_plan( + // auto solution = controller->get_output_port_solution_curr_plan( // ).Eval(*controller_context); - // std::cout << "\nc3_solution.x_sol_ = " << c3_solution.x_sol_ << std::endl; - // std::cout << "\nc3_solution.lambda_sol_ = " << c3_solution.lambda_sol_ + // std::cout << "\nsolution.x_sol_ = " << solution.x_sol_ << std::endl; + // std::cout << "\nsolution.lambda_sol_ = " << solution.lambda_sol_ // << std::endl; - // std::cout << "\nc3_solution.u_sol_ = " << c3_solution.u_sol_ << std::endl; - // std::cout << "\nc3_solution.time_vector_ = " << c3_solution.time_vector_ + // std::cout << "\nsolution.u_sol_ = " << solution.u_sol_ << std::endl; + // std::cout << "\nsolution.time_vector_ = " << solution.time_vector_ // << std::endl; return 0; diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 8122dc1294..ccc2c552d7 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -19,66 +19,6 @@ cc_library( ], ) -cc_library( - name = "c3", - srcs = [ - "base_c3.cc", - "c3_miqp.cc", - "c3_qp.cc", - "c3_plus.cc", - ], - hdrs = [ - "base_c3.h", - "c3_miqp.h", - "c3_options.h", - "c3_qp.h", - "c3_plus.h", - ], - copts = [ - "-fopenmp", - ], - linkopts = [ - "-fopenmp", - ], - deps = [ - ":lcs", - "//solvers:fast_osqp_solver", - "@drake//:drake_shared_library", - "@gurobi//:gurobi_cxx", - ], -) - -cc_library( - name = "c3_output", - srcs = [ - "c3_output.cc", - ], - hdrs = [ - "c3_output.h", - ], - deps = [ - "//lcmtypes:lcmt_robot", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "lcs", - srcs = [ - "lcs.cc", - "lcs_factory.cc", - ], - hdrs = [ - "lcs.h", - "lcs_factory.h", - ], - deps = [ - "//multibody:geom_geom_collider", - "//multibody/kinematic", - "@drake//:drake_shared_library", - ], -) - cc_library( name = "constraint_factory", srcs = [ diff --git a/solvers/base_c3.cc b/solvers/base_c3.cc deleted file mode 100644 index f5e0d81db9..0000000000 --- a/solvers/base_c3.cc +++ /dev/null @@ -1,1039 +0,0 @@ -#include "solvers/base_c3.h" - -#include -#include - -#include -#include - -#include "solvers/lcs.h" - -#include "drake/solvers/mathematical_program.h" -#include "drake/solvers/moby_lcp_solver.h" -#include "drake/solvers/osqp_solver.h" -#include "drake/solvers/solve.h" - -namespace dairlib { -namespace solvers { - -using Eigen::MatrixXd; -using Eigen::VectorXd; -using std::vector; - -using drake::solvers::MathematicalProgram; -using drake::solvers::MathematicalProgramResult; -using drake::solvers::SolutionResult; -using drake::solvers::SolverOptions; - -using drake::solvers::OsqpSolver; -using drake::solvers::OsqpSolverDetails; -using drake::solvers::Solve; - -C3Base::CostMatrices::CostMatrices(const std::vector& Q, - const std::vector& R, - const std::vector& G, - const std::vector& U) { - this->Q = Q; - this->R = R; - this->G = G; - this->U = U; -} - -C3Base::C3Base(const LCS& lcs, const C3Base::CostMatrices& costs, - const vector& x_des, const C3Options& options, - const int z_size) - : warm_start_(options.warm_start), - lcs_(lcs), - N_((lcs.A_).size()), - n_((lcs.A_)[0].cols()), - m_((lcs.D_)[0].cols()), - k_((lcs.B_)[0].cols()), - A_(lcs.A_), - B_(lcs.B_), - D_(lcs.D_), - d_(lcs.d_), - E_(lcs.E_), - F_(lcs.F_), - H_(lcs.H_), - c_(lcs.c_), - Q_(costs.Q), - R_(costs.R), - U_(costs.U), - G_(costs.G), - x_desired_(x_des), - options_(options), - h_is_zero_(lcs.H_[0].isZero(0)), - osqp_(OsqpSolver()), - prog_(MathematicalProgram()), - z_size_(z_size) { - ScaleLCS(); - if (warm_start_) { - InitializeWarmStarts(); - } - InitializeOptimizationVariables(); - InitializeDynamicsConstraints(); - InitializeStateAndInputCosts(); -} - -C3Base::C3Base(const LCS& lcs, const C3Base::CostMatrices& costs, - const std::vector& x_des, - const C3Options& options) - : C3Base(lcs, costs, x_des, options, - lcs.A_[0].cols() + lcs.D_[0].cols() + lcs.B_[0].cols()) {} - -void C3Base::ScaleLCS() { - DRAKE_DEMAND(lcs_.D_.at(0).norm() > 0); - auto Dn = lcs_.D_.at(0).norm(); - auto An = lcs_.A_.at(0).norm(); - AnDn_ = An / Dn; - for (int i = 0; i < N_; ++i) { - D_.at(i) *= AnDn_; - E_.at(i) /= AnDn_; - c_.at(i) /= AnDn_; - H_.at(i) /= AnDn_; - } -} - -void C3Base::InitializeWarmStarts() { - warm_start_delta_.resize(options_.admm_iter + 1); - warm_start_binary_.resize(options_.admm_iter + 1); - warm_start_x_.resize(options_.admm_iter + 1); - warm_start_lambda_.resize(options_.admm_iter + 1); - warm_start_u_.resize(options_.admm_iter + 1); - for (size_t iter = 0; iter < options_.admm_iter + 1; ++iter) { - warm_start_delta_[iter].resize(N_); - for (size_t i = 0; i < N_; i++) { - warm_start_delta_[iter][i] = VectorXd::Zero(z_size_); - } - warm_start_binary_[iter].resize(N_); - for (size_t i = 0; i < N_; i++) { - warm_start_binary_[iter][i] = VectorXd::Zero(m_); - } - warm_start_x_[iter].resize(N_ + 1); - for (size_t i = 0; i < N_ + 1; i++) { - warm_start_x_[iter][i] = VectorXd::Zero(n_); - } - warm_start_lambda_[iter].resize(N_); - for (size_t i = 0; i < N_; i++) { - warm_start_lambda_[iter][i] = VectorXd::Zero(m_); - } - warm_start_u_[iter].resize(N_); - for (size_t i = 0; i < N_; i++) { - warm_start_u_[iter][i] = VectorXd::Zero(k_); - } - } -} - -void C3Base::InitializeOptimizationVariables() { - x_ = vector(); - u_ = vector(); - lambda_ = vector(); - - z_sol_ = std::make_unique>(); - x_sol_ = std::make_unique>(); - lambda_sol_ = std::make_unique>(); - - u_sol_ = std::make_unique>(); - w_sol_ = std::make_unique>(); - delta_sol_ = std::make_unique>(); - for (int i = 0; i < N_; ++i) { - z_sol_->push_back(Eigen::VectorXd::Zero(z_size_)); - x_sol_->push_back(Eigen::VectorXd::Zero(n_)); - lambda_sol_->push_back(Eigen::VectorXd::Zero(m_)); - u_sol_->push_back(Eigen::VectorXd::Zero(k_)); - w_sol_->push_back(Eigen::VectorXd::Zero(z_size_)); - delta_sol_->push_back(Eigen::VectorXd::Zero(z_size_)); - } - - for (int i = 0; i < N_ + 1; i++) { - x_.push_back(prog_.NewContinuousVariables(n_, "x" + std::to_string(i))); - if (i < N_) { - u_.push_back(prog_.NewContinuousVariables(k_, "k" + std::to_string(i))); - lambda_.push_back( - prog_.NewContinuousVariables(m_, "lambda" + std::to_string(i))); - } - } -} - -void C3Base::InitializeDynamicsConstraints() { - MatrixXd LinEq(n_, 2 * n_ + m_ + k_); - LinEq.block(0, n_ + m_ + k_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); - dynamics_constraints_.resize(N_); - for (int i = 0; i < N_; i++) { - LinEq.block(0, 0, n_, n_) = A_.at(i); - LinEq.block(0, n_, n_, m_) = D_.at(i); - LinEq.block(0, n_ + m_, n_, k_) = B_.at(i); - - dynamics_constraints_[i] = - prog_ - .AddLinearEqualityConstraint( - LinEq, -lcs_.d_.at(i), - {x_.at(i), lambda_.at(i), u_.at(i), x_.at(i + 1)}) - .evaluator() - .get(); - } -} - -void C3Base::InitializeStateAndInputCosts() { - target_cost_.resize(N_ + 1); - input_costs_.resize(N_); - for (int i = 0; i < N_ + 1; i++) { - target_cost_[i] = - prog_ - .AddQuadraticCost(2 * Q_.at(i), -2 * Q_.at(i) * x_desired_.at(i), - x_.at(i), 1) - .evaluator() - .get(); - if (i < N_) { - input_costs_[i] = - prog_.AddQuadraticCost(2 * R_.at(i), VectorXd::Zero(k_), u_.at(i), 1) - .evaluator(); - } - } -} - -void C3Base::UpdateCostMatrices(const C3Base::CostMatrices& costs) { - DRAKE_DEMAND(costs.Q.size() == N_ + 1); - DRAKE_DEMAND(costs.R.size() == N_); - DRAKE_DEMAND(costs.U.size() == N_); - DRAKE_DEMAND(costs.G.size() == N_); - DRAKE_DEMAND(costs.Q[0].rows() == n_); - DRAKE_DEMAND(costs.Q[0].cols() == n_); - DRAKE_DEMAND(costs.R[0].rows() == k_); - DRAKE_DEMAND(costs.R[0].cols() == k_); - DRAKE_DEMAND(costs.U[0].rows() == z_size_); - DRAKE_DEMAND(costs.U[0].cols() == z_size_); - DRAKE_DEMAND(costs.G[0].rows() == z_size_); - DRAKE_DEMAND(costs.G[0].cols() == z_size_); - Q_ = costs.Q; - R_ = costs.R; - U_ = costs.U; - G_ = costs.G; -} - -void C3Base::UpdateLCS(const LCS& lcs) { - DRAKE_DEMAND(lcs.A_.size() == N_); - DRAKE_DEMAND(lcs.A_[0].rows() == n_); - DRAKE_DEMAND(lcs.A_[0].cols() == n_); - DRAKE_DEMAND(lcs.D_[0].cols() == m_); - DRAKE_DEMAND(lcs.B_[0].cols() == k_); - // Update the stored LCS object. - lcs_ = lcs; - A_ = lcs.A_; - B_ = lcs.B_; - D_ = lcs.D_; - d_ = lcs.d_; - E_ = lcs.E_; - F_ = lcs.F_; - H_ = lcs.H_; - c_ = lcs.c_; - dt_ = lcs.dt_; - W_x_ = lcs.W_x_; - W_l_ = lcs.W_l_; - W_u_ = lcs.W_u_; - w_ = lcs.w_; - h_is_zero_ = H_[0].isZero(0); - - ScaleLCS(); - UpdateDynamicsConstraints(); -} - -void C3Base::UpdateDynamicsConstraints() { - MatrixXd LinEq = MatrixXd::Zero(n_, n_ + n_ + m_ + k_); - LinEq.block(0, n_ + m_ + k_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); - - for (int i = 0; i < N_; i++) { - LinEq.block(0, 0, n_, n_) = A_.at(i); - LinEq.block(0, n_, n_, m_) = D_.at(i); - LinEq.block(0, n_ + m_, n_, k_) = B_.at(i); - dynamics_constraints_[i]->UpdateCoefficients(LinEq, -d_.at(i)); - } -} - -void C3Base::UpdateCostLCS(const LCS& lcs_for_cost) { - DRAKE_DEMAND(lcs_for_cost.A_.size() == N_ * options_.lcs_dt_resolution); - DRAKE_DEMAND(lcs_for_cost.A_[0].rows() == n_); - DRAKE_DEMAND(lcs_for_cost.A_[0].cols() == n_); - DRAKE_DEMAND(lcs_for_cost.B_[0].cols() == k_); - // Update the stored LCS object. - if (!lcs_for_cost_) { - lcs_for_cost_ = std::make_unique(lcs_for_cost); - } else { - *lcs_for_cost_ = lcs_for_cost; - } -} - -void C3Base::UpdateTarget(const std::vector& x_des) { - x_desired_ = x_des; - for (int i = 0; i < N_ + 1; i++) { - target_cost_[i]->UpdateCoefficients(2 * Q_.at(i), - -2 * Q_.at(i) * x_desired_.at(i)); - } -} - -void C3Base::Solve(const VectorXd& x0, bool verbose) { - auto start = std::chrono::high_resolution_clock::now(); - - VectorXd delta_init = VectorXd::Zero(z_size_); - if (options_.delta_option == 1) { - delta_init.head(n_) = x0; - } - std::vector delta(N_, delta_init); - std::vector w(N_, VectorXd::Zero(z_size_)); - vector Gv = G_; - - for (int i = 0; i < N_; ++i) { - if (options_.penalize_changes_in_u_across_solves) { - // Penalize deviation from previous input solution: input cost is - // (u-u_prev)' * R * (u-u_prev). - input_costs_[i]->UpdateCoefficients(2 * R_.at(i), - -2 * R_.at(i) * u_sol_->at(i)); - } else { - // Penalize inputs: input cost is u' * R * u. - input_costs_[i]->UpdateCoefficients(2 * R_.at(i), - Eigen::VectorXd::Zero(k_)); - } - } - - auto admm_start = std::chrono::high_resolution_clock::now(); - for (int iter = 0; iter < options_.admm_iter; iter++) { - ADMMStep(x0, &delta, &w, &Gv, iter, verbose); - } - auto admm_end = std::chrono::high_resolution_clock::now(); - std::chrono::duration duration_ms = admm_end - admm_start; - //std::cout << "ADMM: " << duration_ms.count() << " ms" << std::endl; - - - - vector WD(N_, VectorXd::Zero(z_size_)); - for (int i = 0; i < N_; i++) { - WD.at(i) = delta.at(i) - w.at(i); - } - - vector zfin = SolveQP(x0, Gv, WD, delta, options_.admm_iter, true); - - if (verbose) { - std::cout << "x0: " << x0.transpose() << std::endl; - std::cout << "Final ADMM Iteration: " << options_.admm_iter << std::endl; - // Make matrix versions of variables for more compact printing. - Eigen::MatrixXd verbose_delta = Eigen::MatrixXd::Zero(z_size_, N_); - Eigen::MatrixXd verbose_w = Eigen::MatrixXd::Zero(z_size_, N_); - Eigen::MatrixXd verbose_zfin = Eigen::MatrixXd::Zero(z_size_, N_); - Eigen::MatrixXd verbose_zsol = Eigen::MatrixXd::Zero(z_size_, N_); - for (int i = 0; i < N_; i++) { - verbose_delta.col(i) = delta[i]; - verbose_w.col(i) = w[i]; - verbose_zfin.col(i) = zfin_[i]; - verbose_zsol.col(i) = z_sol_->at(i); - } - std::cout << "zfin: \n" << verbose_zfin << std::endl; - std::cout << "zsol: \n" << verbose_zsol << std::endl; - std::cout << "delta: \n" << verbose_delta << std::endl; - std::cout << "w: \n" << verbose_w << std::endl; - } - - *w_sol_ = w; - *delta_sol_ = delta; - - if (!options_.end_on_qp_step) { - *z_sol_ = delta; - z_sol_->at(0).segment(0, n_) = x0; - x_sol_->at(0) = x0; - for (int i = 1; i < N_; ++i) { - z_sol_->at(i).segment(0, n_) = - A_.at(i - 1) * x_sol_->at(i - 1) + B_.at(i - 1) * u_sol_->at(i - 1) + - D_.at(i - 1) * lambda_sol_->at(i - 1) + d_.at(i - 1); - } - } - - for (int i = 0; i < N_; ++i) { - lambda_sol_->at(i) *= AnDn_; - z_sol_->at(i).segment(n_, m_) *= AnDn_; - } - - zfin_ = *z_sol_; - auto finish = std::chrono::high_resolution_clock::now(); - auto elapsed = finish - start; - solve_time_ = - std::chrono::duration_cast(elapsed).count() / - 1e6; -} - -// // This function relies on the previously computed zfin_ from Solve. -// std::pair> C3Base::CalcCost( -// C3CostComputationType cost_type, -// std::vector Kp_for_ee_pd_rollout, -// std::vector Kd_for_ee_pd_rollout, -// bool force_tracking_disabled, -// int num_objects, -// bool print_cost_breakdown, -// bool verbose) const { - -// int resolution = options_.lcs_dt_resolution; - -// std::vector UU(N_*resolution, VectorXd::Zero(k_)); -// std::vector XX(N_*resolution + 1, VectorXd::Zero(n_)); - -// const int ee_vel_index = 7 * num_objects + 3; -// // Simulate the dynamics from the planned inputs. -// if (cost_type == C3CostComputationType::kSimLCS) { -// XX[0] = zfin_[0].segment(0, n_); -// for (int i = 0; i < N_ * resolution; i++) { -// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); -// if (lcs_for_cost_) { -// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); -// } -// else { -// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); -// } -// } -// } - -// // Use the C3 plan. -// else if (cost_type == C3CostComputationType::kUseC3Plan) { -// for (int i = 0; i < N_ * resolution; i++) { -// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); -// XX[i] = zfin_[i / resolution].segment(0, n_); -// if (i == N_-1) { -// if (lcs_for_cost_) { -// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); -// } -// else { -// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); -// } -// } -// } -// } - -// // Simulate the dynamics from the planned inputs only for the object; use the -// // planned EE trajectory. -// else if (cost_type == C3CostComputationType::kSimLCSReplaceC3EEPlan) { -// // Simulate the object trajectory. -// XX[0] = zfin_[0].segment(0, n_); -// for (int i = 0; i < N_ * resolution; i++) { -// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); -// if (lcs_for_cost_) { -// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); -// } -// else { -// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); -// } -// } -// // Replace ee traj with those from zfin_. -// for (int i = 0; i < N_; i++) { -// XX[i].segment(0,3) = zfin_[i / resolution].segment(0,3); -// if (i == N_-1) { -// if (lcs_for_cost_) { -// XX[i+1].segment(0,3) = -// lcs_for_cost_->Simulate(XX[i], UU[i]).segment(0,3); -// } -// else { -// XX[i+1].segment(0,3) = lcs_.Simulate(XX[i], UU[i]).segment(0,3); -// } -// } -// } -// } - -// // Try to emulate the real cost of the system associated not only applying the -// // planned u but also the u associated with tracking the position plan over -// // time. -// else if (cost_type == C3CostComputationType::kSimImpedance) { -// std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, -// Kd_for_ee_pd_rollout, -// num_objects, -// force_tracking_disabled, -// verbose); -// } - -// // The same as the previous cost type except the EE states are replaced with -// // the plan from C3 at the end. -// else if (cost_type == C3CostComputationType::kSimImpedanceReplaceC3EEPlan) { -// std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, -// Kd_for_ee_pd_rollout, -// num_objects, -// force_tracking_disabled, -// verbose); - -// // Replace the end effector position and velocity plans with the ones from -// // the C3 plan. -// for(int i = 0; i < N_*resolution; i++) { -// XX[i].segment(0,3) = zfin_[i].segment(0,3); -// XX[i].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); -// if (i == N_*resolution-1) { -// XX[i+1].segment(0,3) = zfin_[i].segment(0,3); -// XX[i+1].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); -// } -// } -// } - -// // The same as the previous cost type except only object terms contribute to -// // the final cost. -// else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { -// std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, -// Kd_for_ee_pd_rollout, -// num_objects, -// force_tracking_disabled, -// verbose); -// } - - -// // Declare Q_eff and R_eff as the Q and R to use for cost computation. -// std::vector Q_eff = Q_; -// std::vector R_eff = R_; - -// // Calculate the cost over the N+1 time steps. -// double cost = 0; - -// //used only for verbose mode printouts -// double error_contrib_ee_pos = 0; -// double error_contrib_obj_orientation = 0; -// double error_contrib_obj_pos = 0; -// double error_contrib_ee_vel = 0; -// double error_contrib_obj_ang_vel = 0; -// double error_contrib_obj_vel = 0; - -// double cost_contrib_ee_pos = 0; -// double cost_contrib_obj_orientation = 0; -// double cost_contrib_obj_pos = 0; -// double cost_contrib_ee_vel = 0; -// double cost_contrib_obj_ang_vel = 0; -// double cost_contrib_obj_vel = 0; - -// int obj_orientation_index = 0; -// int obj_pos_index = 0; -// int obj_ang_vel_index = 0; -// int obj_vel_index = 0; - -// double cost_contrib_u = 0; - -// // Calculate the error and cost contributions for each state. -// for (int i = 0; i < N_*resolution; i+=resolution) { -// //ee_pos -// error_contrib_ee_pos += -// (XX[i].segment(0,3) - x_desired_[i / resolution].segment(0,3)).transpose() * -// (XX[i].segment(0,3) - x_desired_[i / resolution].segment(0,3)); -// cost_contrib_ee_pos += -// (XX[i].segment(0,3) - x_desired_[i / resolution].segment(0,3)).transpose() * -// Q_eff.at(i / resolution).block(0,0,3,3)*(XX[i].segment(0,3) - -// x_desired_[i / resolution].segment(0,3)); -// //obj_orientation -// for (int j = 0; j < num_objects; j++) { -// obj_orientation_index = 7*j + 3; -// obj_pos_index = 7*j + 7; -// error_contrib_obj_orientation += -// (XX[i].segment(obj_orientation_index,4) - x_desired_[i / resolution].segment(obj_orientation_index,4)).transpose() * -// (XX[i].segment(obj_orientation_index,4) - x_desired_[i / resolution].segment(obj_orientation_index,4)); -// cost_contrib_obj_orientation += -// (XX[i].segment(obj_orientation_index,4) - x_desired_[i / resolution].segment(obj_orientation_index,4)).transpose() * -// Q_eff.at(i / resolution).block(obj_orientation_index,obj_orientation_index,4,4) * -// (XX[i].segment(obj_orientation_index,4) - x_desired_[i / resolution].segment(obj_orientation_index,4)); -// //obj_pos -// error_contrib_obj_pos += -// (XX[i].segment(obj_pos_index,3) - x_desired_[i / resolution].segment(obj_pos_index,3)).transpose() * -// (XX[i].segment(obj_pos_index,3) - x_desired_[i / resolution].segment(obj_pos_index,3)); -// cost_contrib_obj_pos += -// (XX[i].segment(obj_pos_index,3) - x_desired_[i / resolution].segment(obj_pos_index,3)).transpose() * -// Q_eff.at(i / resolution).block(obj_pos_index,obj_pos_index,3,3)*(XX[i].segment(obj_pos_index,3) - -// x_desired_[i / resolution].segment(obj_pos_index,3)); -// } -// //ee_vel -// error_contrib_ee_vel += -// (XX[i].segment(ee_vel_index,3) - x_desired_[i / resolution].segment(ee_vel_index,3)).transpose() * -// (XX[i].segment(ee_vel_index,3) - x_desired_[i / resolution].segment(ee_vel_index,3)); -// cost_contrib_ee_vel += -// (XX[i].segment(ee_vel_index,3) - x_desired_[i / resolution].segment(ee_vel_index,3)).transpose() * -// Q_eff.at(i / resolution).block(ee_vel_index,ee_vel_index,3,3) * (XX[i].segment(ee_vel_index,3) - -// x_desired_[i / resolution].segment(ee_vel_index,3)); -// //obj_ang_vel -// for (int j = 0; j < num_objects; j++) { -// obj_ang_vel_index = 6*j + 6 + 7*num_objects; -// obj_vel_index = 6*j + 9 + 7*num_objects; -// error_contrib_obj_ang_vel += -// (XX[i].segment(obj_ang_vel_index,3) - x_desired_[i / resolution].segment(obj_ang_vel_index,3)).transpose() * -// (XX[i].segment(obj_ang_vel_index,3) - x_desired_[i / resolution].segment(obj_ang_vel_index,3)); -// cost_contrib_obj_ang_vel += -// (XX[i].segment(obj_ang_vel_index,3) - x_desired_[i / resolution].segment(obj_ang_vel_index,3)).transpose() * -// Q_eff.at(i / resolution).block(obj_ang_vel_index,obj_ang_vel_index,3,3)*(XX[i].segment(obj_ang_vel_index,3) - -// x_desired_[i / resolution].segment(obj_ang_vel_index,3)); -// //obj_vel -// error_contrib_obj_vel += -// (XX[i].segment(obj_vel_index,3) - x_desired_[i / resolution].segment(obj_vel_index,3)).transpose() * -// (XX[i].segment(obj_vel_index,3) - x_desired_[i / resolution].segment(obj_vel_index,3)); -// cost_contrib_obj_vel += -// (XX[i].segment(obj_vel_index,3) - x_desired_[i / resolution].segment(obj_vel_index,3)).transpose() * -// Q_eff.at(i / resolution).block(obj_vel_index,obj_vel_index,3,3)*(XX[i].segment(obj_vel_index,3) - -// x_desired_[i / resolution].segment(obj_vel_index,3)); -// } - -// cost = cost + (XX[i] - x_desired_[i / resolution]).transpose() * -// Q_eff.at(i / resolution)*(XX[i] - x_desired_[i / resolution]) + -// UU[i].transpose()*R_eff.at(i / resolution)*UU[i]; - -// cost_contrib_u += UU[i].transpose()*R_eff.at(i / resolution)*UU[i]; - -// } - -// DRAKE_DEMAND(!std::isnan(cost_contrib_obj_vel)); - -// // Handle the N_th state. -// cost = cost + (XX[N_] - x_desired_[N_]).transpose()*Q_eff.at(N_)*( -// XX[N_] - x_desired_[N_]); - -// error_contrib_ee_pos += -// (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)).transpose() * -// (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)); -// for (int j = 0; j < num_objects; j++) { -// obj_orientation_index = 7*j + 3; -// obj_pos_index = 7*j + 7; -// error_contrib_obj_orientation += -// (XX[N_].segment(obj_orientation_index,4) - x_desired_[N_].segment(obj_orientation_index,4)).transpose() * -// (XX[N_].segment(obj_orientation_index,4) - x_desired_[N_].segment(obj_orientation_index,4)); -// error_contrib_obj_pos += -// (XX[N_].segment(obj_pos_index,3) - x_desired_[N_].segment(obj_pos_index,3)).transpose() * -// (XX[N_].segment(obj_pos_index,3) - x_desired_[N_].segment(obj_pos_index,3)); -// } -// error_contrib_ee_vel += -// (XX[N_].segment(ee_vel_index,3) - x_desired_[N_].segment(ee_vel_index,3)).transpose() * -// (XX[N_].segment(ee_vel_index,3) - x_desired_[N_].segment(ee_vel_index,3)); - -// for (int j = 0; j < num_objects; j++) { -// obj_ang_vel_index = 6*j + 6 + 7*num_objects; -// obj_vel_index = 6*j + 9 + 7*num_objects; -// error_contrib_obj_ang_vel += -// (XX[N_].segment(obj_ang_vel_index,3) - x_desired_[N_].segment(obj_ang_vel_index,3)).transpose() * -// (XX[N_].segment(obj_ang_vel_index,3) - x_desired_[N_].segment(obj_ang_vel_index,3)); -// error_contrib_obj_vel += -// (XX[N_].segment(obj_vel_index,3) - x_desired_[N_].segment(obj_vel_index,3)).transpose() * -// (XX[N_].segment(obj_vel_index,3) - x_desired_[N_].segment(obj_vel_index,3)); -// } - -// cost_contrib_ee_pos += -// (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)).transpose() * -// Q_eff.at(N_).block(0,0,3,3) * -// (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)); -// for (int j = 0; j < num_objects; j++) { -// obj_orientation_index = 7*j + 3; -// obj_pos_index = 7*j + 7; -// cost_contrib_obj_orientation += -// (XX[N_].segment(obj_orientation_index,4) - x_desired_[N_].segment(obj_orientation_index,4)).transpose() * -// Q_eff.at(N_).block(obj_orientation_index,obj_orientation_index,4,4)*(XX[N_].segment(obj_orientation_index,4) - -// x_desired_[N_].segment(obj_orientation_index,4)); -// cost_contrib_obj_pos += -// (XX[N_].segment(obj_pos_index,3) - x_desired_[N_].segment(obj_pos_index,3)).transpose() * -// Q_eff.at(N_).block(obj_pos_index,obj_pos_index,3,3)*(XX[N_].segment(obj_pos_index,3) - -// x_desired_[N_].segment(obj_pos_index,3)); -// } -// cost_contrib_ee_vel += -// (XX[N_].segment(ee_vel_index,3) - x_desired_[N_].segment(ee_vel_index,3)).transpose() * -// Q_eff.at(N_).block(ee_vel_index,ee_vel_index,3,3)*(XX[N_].segment(ee_vel_index,3) - -// x_desired_[N_].segment(ee_vel_index,3)); -// for (int j = 0; j < num_objects; j++) { -// obj_ang_vel_index = 6*j + 6 + 7*num_objects; -// obj_vel_index = 6*j + 9 + 7*num_objects; -// cost_contrib_obj_ang_vel += -// (XX[N_].segment(obj_ang_vel_index,3) - x_desired_[N_].segment(obj_ang_vel_index,3)).transpose() * -// Q_eff.at(N_).block(obj_ang_vel_index,obj_ang_vel_index,3,3)*(XX[N_].segment(obj_ang_vel_index,3) - -// x_desired_[N_].segment(obj_ang_vel_index,3)); -// cost_contrib_obj_vel += -// (XX[N_].segment(obj_vel_index,3) - x_desired_[N_].segment(obj_vel_index,3)).transpose() * -// Q_eff.at(N_).block(obj_vel_index,obj_vel_index,3,3)*(XX[N_].segment(obj_vel_index,3) - -// x_desired_[N_].segment(obj_vel_index,3)); -// } - -// if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { -// cost = cost_contrib_obj_pos + cost_contrib_obj_orientation + -// cost_contrib_obj_vel + cost_contrib_obj_ang_vel; -// } - -// if (verbose || print_cost_breakdown) { -// std::cout<<"Error breakdown"< XX_downsampled(N_ + 1, VectorXd::Zero(n_)); - -// for (int i = 0; i < N_*resolution; i += resolution) { -// XX_downsampled[i/resolution] = XX[i]; -// } -// XX_downsampled[N_] = XX[N_*resolution]; - -// // for (int i = 0; i < XX_downsampled.size(); i++) { -// // std::cout << XX_downsampled[i].transpose() << std::endl; -// // } -// //std::cout << "\n\n" << std::endl; - -// std::pair > ret (cost, XX_downsampled); -// // for (int j =0; j <= N_; j++) { -// // std::cout << XX[j].transpose() << std::endl; -// // } -// // std::cout << "\n\n" << std::endl; -// return ret; -// } - -// std::pair, std::vector> -// C3Base::SimulatePDControl( -// std::vector Kp_for_ee_pd_rollout, std::vector Kd_for_ee_pd_rollout, int num_objects, -// bool force_tracking_disabled, bool verbose) const -// { -// if (verbose) { -// std::cout<<"\nSIMULATING PD CONTROL"< UU(N_*resolution, VectorXd::Zero(k_)); -// std::vector XX(N_*resolution +1, VectorXd::Zero(n_)); -// for (int i = 0; i < N_*resolution; i++) { -// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); -// XX[i] = zfin_[i / resolution].segment(0, n_); -// if (i == N_*resolution-1) { -// if (lcs_for_cost_) { -// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); -// } -// else{ -// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); -// } -// } -// } - -// // Smooth XX and UU -// // for (int i = 0; i < N_-1; i++) { -// // Eigen::VectorXd UU_start = UU[i * resolution]; -// // Eigen::VectorXd UU_end = UU[(i+1) * resolution]; -// // Eigen::VectorXd XX_start = XX[i * resolution]; -// // Eigen::VectorXd XX_end = XX[(i+1) * resolution]; - -// // Eigen::VectorXd UU_diff = UU_end - UU_start; -// // Eigen::VectorXd XX_diff = XX_end - XX_start; - -// // for (int j = 0; j < resolution; j++) { -// // UU[i * resolution + j] = UU[i*resolution] + (j / resolution) * UU_diff; -// // XX[i * resolution + j] = XX[i*resolution] + (j / resolution) * XX_diff; -// // } -// // } - -// // Set the PD gains for the emulated tracking controller. -// Eigen::VectorXd Kp_vector = Eigen::Map(Kp_for_ee_pd_rollout.data(), Kp_for_ee_pd_rollout.size()); -// Eigen::VectorXd Kd_vector = Eigen::Map(Kd_for_ee_pd_rollout.data(), Kd_for_ee_pd_rollout.size()); - - -// Eigen::MatrixXd Kp = Kp_vector.asDiagonal(); -// Eigen::MatrixXd Kd = Kd_vector.asDiagonal(); - - -// // Obtain modified solutions for the PD controller. -// std::vector UU_new(N_*resolution, VectorXd::Zero(k_)); -// std::vector XX_new(N_*resolution+1, VectorXd::Zero(n_)); - -// XX_new[0] = zfin_[0].segment(0, n_); -// // This will just be the original u from zfin_[0] for the first time step. -// // if the radio input is true, then the u will only emulate position -// // tracking using the PD controller. - -// for (int i = 0; i < N_*resolution; i++) { - -// if (force_tracking_disabled) { -// UU_new[i] = Kp*(XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + -// Kd*(XX[i].segment(ee_vel_index, 3) - XX_new[i].segment(ee_vel_index, 3)); -// } -// else{ -// UU_new[i] = UU[i] + -// Kp*(XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + -// Kd*(XX[i].segment(ee_vel_index, 3) - XX_new[i].segment(ee_vel_index, 3)); -// } - -// if (lcs_for_cost_) { - -// if (verbose) { -// std::cout<<"simulated step "<Simulate(XX_new[i], UU_new[i], verbose); -// } -// else { -// XX_new[i+1] = lcs_.Simulate(XX_new[i], UU_new[i]); -// } -// } -// return {XX_new, UU_new}; -// } - - -void C3Base::ADMMStep(const VectorXd& x0, vector* delta, - vector* w, vector* Gv, - int admm_iteration, bool verbose) { - vector WD(N_, VectorXd::Zero(z_size_)); - - for (int i = 0; i < N_; i++) { - WD.at(i) = delta->at(i) - w->at(i); - } - - vector z = SolveQP(x0, *Gv, WD, *delta, admm_iteration, false); - if (verbose) { - std::cout << "SolveQP Iteration: " << admm_iteration << std::endl; - Eigen::MatrixXd verbose_z = Eigen::MatrixXd::Zero(z_size_, N_); - for (int i = 0; i < N_; i++) { - verbose_z.col(i) = z[i]; - } - std::cout << "z: \n" << verbose_z << std::endl; - } - - vector ZW(N_, VectorXd::Zero(z_size_)); - for (int i = 0; i < N_; i++) { - ZW[i] = w->at(i) + z[i]; - } - - if (U_[0].isZero(0)) { - *delta = SolveProjection(*Gv, ZW, admm_iteration); - - } else { - *delta = SolveProjection(U_, ZW, admm_iteration); - } - if (verbose) { - std::cout << "ADMM Iteration: " << admm_iteration << std::endl; - Eigen::MatrixXd verbose_delta = Eigen::MatrixXd::Zero(z_size_, N_); - for (int i = 0; i < N_; i++) { - verbose_delta.col(i) = delta->at(i); - } - std::cout << "delta: \n" << verbose_delta << std::endl; - } - - for (int i = 0; i < N_; i++) { - w->at(i) = w->at(i) + z[i] - delta->at(i); - w->at(i) = w->at(i) / options_.rho_scale; - Gv->at(i) = Gv->at(i) * options_.rho_scale; - } -} - -void C3Base::AddAugmentedCostsQPStep(const vector& G, - const vector& WD, - const vector& delta, - bool is_final_solve) { - for (int i = 0; i < N_; i++) { - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(n_, n_, m_, m_), - -2 * G.at(i).block(n_, n_, m_, m_) * WD.at(i).segment(n_, m_), - lambda_.at(i), 1)); - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(0, 0, n_, n_), - -2 * G.at(i).block(0, 0, n_, n_) * WD.at(i).segment(0, n_), x_.at(i), - 1)); - costs_.push_back( - prog_.AddQuadraticCost(2 * G.at(i).block(n_ + m_, n_ + m_, k_, k_), - -2 * G.at(i).block(n_ + m_, n_ + m_, k_, k_) * - WD.at(i).segment(n_ + m_, k_), - u_.at(i), 1)); - } -} - -void C3Base::SetInitialGuessQPStep(const Eigen::VectorXd& x0, - int admm_iteration) { - if (warm_start_) { - int index = solve_time_ / dt_; - double weight = (solve_time_ - index * dt_) / dt_; - for (int i = 0; i < N_ - 1; i++) { - prog_.SetInitialGuess(x_[i], - (1 - weight) * warm_start_x_[admm_iteration][i] + - weight * warm_start_x_[admm_iteration][i + 1]); - prog_.SetInitialGuess( - lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + - weight * warm_start_lambda_[admm_iteration][i + 1]); - prog_.SetInitialGuess(u_[i], - (1 - weight) * warm_start_u_[admm_iteration][i] + - weight * warm_start_u_[admm_iteration][i + 1]); - } - prog_.SetInitialGuess(x_[0], x0); - prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); - } -} - -void C3Base::ExtractQPSolution( - const drake::solvers::MathematicalProgramResult& result, int admm_iteration, - bool is_final_solve) { - for (int i = 0; i < N_; i++) { - if (is_final_solve) { - x_sol_->at(i) = result.GetSolution(x_[i]); - lambda_sol_->at(i) = result.GetSolution(lambda_[i]); - u_sol_->at(i) = result.GetSolution(u_[i]); - } - z_sol_->at(i).segment(0, n_) = result.GetSolution(x_[i]); - z_sol_->at(i).segment(n_, m_) = result.GetSolution(lambda_[i]); - z_sol_->at(i).segment(n_ + m_, k_) = result.GetSolution(u_[i]); - } -} - -void C3Base::UpdateWarmStarts( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration) { - for (int i = 0; i < N_; i++) { - warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); - warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]); - warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]); - } - warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); -} - -vector C3Base::SolveQP(const VectorXd& x0, const vector& G, - const vector& WD, const vector& delta, - int admm_iteration, - bool is_final_solve) { - // Remove initial state and initial force constraint from previous solve - for (auto& constraint : constraints_) { - prog_.RemoveConstraint(constraint); - } - constraints_.clear(); - - // Add initial state constraint - constraints_.push_back(prog_.AddLinearConstraint(x_[0] == x0)); - - // H matrix does not depend on u, so just simulate passive system - if (h_is_zero_ == 1) { - drake::solvers::MobyLCPSolver LCPSolver; - VectorXd lambda0; - LCPSolver.SolveLcpLemkeRegularized(F_[0], E_[0] * x0 + c_[0], &lambda0); - constraints_.push_back(prog_.AddLinearConstraint(lambda_[0] == lambda0)); - } - - // Remove augmented costs from previous solve - for (auto& cost : costs_) { - prog_.RemoveCost(cost); - } - costs_.clear(); - - AddAugmentedCostsQPStep(G, WD, delta, is_final_solve); - - SetInitialGuessQPStep(x0, admm_iteration); - - // for (const auto& binding : prog_.GetAllConstraints()) { - // std::cout << "Constraint type: " - // << binding.evaluator()->get_description() << "\n"; - // try { - // // Try to cast to a LinearConstraint to get A, l, u - // auto lc = dynamic_cast( - // binding.evaluator().get()); - // if (lc) { - // std::cout << "A =\n" << lc->GetDenseA() << "\n"; - // std::cout << "Lower bound = " << lc->lower_bound().transpose() << "\n"; - // std::cout << "Upper bound = " << lc->upper_bound().transpose() << "\n"; - // } - // } catch (...) { - // std::cout << " (not a LinearConstraint)\n"; - // } - - // std::cout << "Variables: " << binding.variables() << "\n"; - // std::cout << "-----------------------------\n"; - // } - - - MathematicalProgramResult result = osqp_.Solve(prog_); - - if (!result.is_success()) { - std::cout << "CAUTION: QP Step in C3 did not succeed" << std::endl; - } - if (warm_start_) { - UpdateWarmStarts(result, admm_iteration); - } - ExtractQPSolution(result, admm_iteration, is_final_solve); - return *z_sol_; -} - -vector C3Base::SolveProjection(const vector& U, - vector& WZ, - int admm_iteration) { - vector deltaProj(N_, VectorXd::Zero(z_size_)); - int i; - - if (options_.num_threads > 0) { - omp_set_dynamic(0); // Explicitly disable dynamic teams - omp_set_num_threads(options_.num_threads); // Set number of threads - // TODO: A newer version of OpenMP likely uses omp_set_max_active_levels - // instead of omp_set_nested. Consider updating this after testing. - omp_set_nested(1); // Enable nested parallelism (important for - // sampling_c3_controller) - } - -#pragma omp parallel for num_threads( \ - options_.num_threads) if (use_parallelization_in_projection_) - for (i = 0; i < N_; i++) { - if (warm_start_) { - if (i == N_ - 1) { - deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], - c_[i], admm_iteration, -1); - } else { - deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], - c_[i], admm_iteration, i + 1); - } - } else { - deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], - c_[i], admm_iteration, -1); - } - } - return deltaProj; -} - -void C3Base::AddLinearConstraint(Eigen::RowVectorXd& A, double lower_bound, - double upper_bound, int constraint) { - if (constraint == 1) { - for (int i = 1; i < N_; i++) { - user_constraints_.push_back( - prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); - } - } - - if (constraint == 2) { - for (int i = 0; i < N_; i++) { - user_constraints_.push_back( - prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); - } - } - - if (constraint == 3) { - for (int i = 0; i < N_; i++) { - user_constraints_.push_back(prog_.AddLinearConstraint( - A, lower_bound, upper_bound, lambda_.at(i))); - } - } -} - -void C3Base::RemoveUserConstraints() { - for (auto& userconstraint : user_constraints_) { - prog_.RemoveConstraint(userconstraint); - } - user_constraints_.clear(); -} - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/base_c3.h b/solvers/base_c3.h deleted file mode 100644 index c643526c44..0000000000 --- a/solvers/base_c3.h +++ /dev/null @@ -1,279 +0,0 @@ -#pragma once - -#include - -#include - -#include "solvers/c3_options.h" -#include "solvers/fast_osqp_solver.h" -#include "solvers/lcs.h" - -#include "drake/solvers/mathematical_program.h" -#include "drake/solvers/osqp_solver.h" -#include "drake/solvers/solve.h" - -namespace dairlib { -namespace solvers { - -class C3Base { - public: - struct CostMatrices { - CostMatrices() = default; - CostMatrices(const std::vector& Q, - const std::vector& R, - const std::vector& G, - const std::vector& U); - std::vector Q; - std::vector R; - std::vector G; - std::vector U; - }; - - /// @param lcs Parameters defining the LCS (Linear Complementarity - /// System). - /// @param costs Cost matrices used in the optimization. - /// @param x_des Desired goal state. - /// @param options Options specific to the C3 formulation. - /// @note Using this constructor will set z_size to the default value, which - /// is size_x + size_u + size_lambda - C3Base(const LCS& LCS, const CostMatrices& costs, - const std::vector& x_des, const C3Options& options); - - virtual ~C3Base() = default; - - /// Solve the MPC problem. - /// @param x0 The initial state of the system - /// @param verbose Whether to print additional information - /// @return void - void Solve(const Eigen::VectorXd& x0, bool verbose = false); - -// /// Compute the MPC cost, using previously solved MPC solution. -// /// @param cost_type The method of computing the cost -// /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control -// /// used for some of the cost types -// /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control -// /// used for some of the cost types -// /// @param force_tracking_disabled Whether to simulate EE PD control with -// /// feedforward u from the MPC solution -// /// @param print_cost_breakdown Whether to print the cost breakdown -// /// @param verbose Whether to print additional information -// /// @return The cost and its associated state trajectory -// std::pair> CalcCost( -// C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan, -// std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, -// std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, -// bool force_tracking_disabled = false, int num_objects = 1, -// bool print_cost_breakdown = false, bool verbose = false) const; - -// /// Helper function to simulate the dynamics with PD control on the EE -// /// location and velocity plans, and the control input plans. Used for cost -// /// types that simulate the impedance control. -// /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control -// /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control -// /// @param force_tracking_disabled Whether to simulate EE PD control with -// /// feedforward u from the MPC solution -// /// @param verbose Whether to print additional information -// /// @return the simulated state and input trajectories -// std::pair, std::vector> -// SimulatePDControl(std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, -// std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, int num_objects = 1, -// bool force_tracking_disabled = false, -// bool verbose = false) const; - - /// Solve a single ADMM step. - /// @param x0 The initial state of the system - /// @param delta The copy variables from the previous step - /// @param w The scaled dual variables from the previous step - /// @param G A pointer to the G variables from previous step - /// @param admm_iteration ADMM iteration for accurate warm starting - /// @param verbose Whether to print additional information - /// @return solution is saved in C3 object - void ADMMStep(const Eigen::VectorXd& x0, std::vector* delta, - std::vector* w, - std::vector* G, int admm_iteration, - bool verbose = false); - - /// Solve a single QP. - /// @param x0 The initial state of the system - /// @param G A pointer to the G variables from previous step - /// @param WD A pointer to the (w - delta) variables - /// @param admm_iteration ADMM iteration for accurate warm starting - /// @param is_final_solve Indicating final admm iteration in case of any - /// polishing steps - /// @return z MPC solution - std::vector SolveQP(const Eigen::VectorXd& x0, - const std::vector& G, - const std::vector& WD, - const std::vector& delta, - int admm_iteration, - bool is_final_solve = false); - - /// Solve the projection problem for all timesteps. - /// @param U Matrix for consensus cost - /// @param WZ (z + w) variables - /// @param admm_iteration ADMM iteration for accurate warm starting - std::vector SolveProjection( - const std::vector& U, std::vector& WZ, - int admm_iteration); - - /// allow users to add constraints (adds for all timesteps) - /// @param A, lower_bound, upper_bound lower_bound <= A^T x <= upper_bound - /// @param constraint inputconstraint, stateconstraint, forceconstraint - void AddLinearConstraint(Eigen::RowVectorXd& A, double lower_bound, - double upper_bound, int constraint); - - /// remove all constraints - void RemoveUserConstraints(); - - /// Solve a projection step for a single knot point k. - /// @param U Matrix for consensus cost - /// @param delta_c A pointer to the copy of (z + w) variables - /// @param E, F, H, c LCS contact parameters - /// @param admm_iteration ADMM iteration for accurate warm starting - /// @param warm_start_index knot point index for warm starting - /// @return delta_k - virtual Eigen::VectorXd SolveSingleProjection( - const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, - const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, - const Eigen::MatrixXd& H, const Eigen::VectorXd& c, - const int admm_iteration, const int& warm_start_index) = 0; - - void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options) { - prog_.SetSolverOptions(options); - } - - std::vector GetFullSolution() { return *z_sol_; } - std::vector GetStateSolution() { return *x_sol_; } - std::vector GetForceSolution() { return *lambda_sol_; } - std::vector GetInputSolution() { return *u_sol_; } - std::vector GetDualDeltaSolution() { return *delta_sol_; } - std::vector GetDualWSolution() { return *w_sol_; } - - int GetZSize() { return z_size_; } - - void UpdateCostMatrices(const C3Base::CostMatrices& costs); - virtual void UpdateLCS(const LCS& lcs); - - /// Update the LCS used for cost computation. This can differ from the LCS - /// used for the solve, e.g. if more contacts are used for cost than for - /// computing the plan. NOTE: This does not update the internal cost - /// matrices used for the solve (Q_, R_, G_, U_). Those are updated via - /// UpdateCostMatrices. - void UpdateCostLCS(const LCS& lcs_for_cost); - void UpdateTarget(const std::vector& x_des); - - protected: - /// @param lcs Parameters defining the LCS. - /// @param costs Cost matrices used in the optimization. - /// @param x_des Desired goal state trajectory. - /// @param options Options specific to the C3 formulation. - /// @param z_size Size of the z vector, which depends on the specific C3 - /// variant. - /// For example: - /// - C3MIQP / C3QP: z = [x, u, lambda] - /// - C3Plus: z = [x, u, lambda, eta] - /// - /// This constructor is intended for internal use only. The public constructor - /// delegates to this one, passing in an explicitly computed z vector size. - C3Base(const LCS& lcs, const CostMatrices& costs, - const std::vector& x_des, const C3Options& options, - int z_size); - - // Helper functions for C3Base constructor - void ScaleLCS(); - void InitializeWarmStarts(); - void InitializeOptimizationVariables(); - void InitializeDynamicsConstraints(); - void InitializeStateAndInputCosts(); - - void UpdateDynamicsConstraints(); - - // Helper functions for QP step - virtual void AddAugmentedCostsQPStep(const std::vector& G, - const std::vector& WD, - const std::vector& delta, - bool is_final_solve); - virtual void SetInitialGuessQPStep(const Eigen::VectorXd& x0, - int admm_iteration); - virtual void ExtractQPSolution( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve); - virtual void UpdateWarmStarts( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration); - - std::vector> warm_start_delta_; - std::vector> warm_start_binary_; - std::vector> warm_start_x_; - std::vector> warm_start_lambda_; - std::vector> warm_start_u_; - bool warm_start_; - const int N_; - const int n_; // n_x - const int m_; // n_lambda - const int k_; // n_u - const int z_size_; - const C3Options options_; - bool use_parallelization_in_projection_ = true; - - // TODO: storing the LCS as a class variable makes the LCS matrices - // redundant. Could consider removing LCS matrices as class variables. - mutable LCS lcs_; - std::unique_ptr lcs_for_cost_; - std::vector A_; - std::vector B_; - std::vector D_; - std::vector d_; - std::vector E_; - std::vector F_; - std::vector H_; - std::vector c_; - Eigen::MatrixXd W_x_; - Eigen::MatrixXd W_l_; - Eigen::MatrixXd W_u_; - Eigen::VectorXd w_; - double AnDn_ = 1.0; - std::vector Q_; - std::vector R_; - std::vector U_; - std::vector G_; - std::vector x_desired_; - double dt_ = 0; - double solve_time_ = 0; - bool h_is_zero_; - - /// MathematicalProgram for QP step - drake::solvers::OsqpSolver osqp_; - drake::solvers::MathematicalProgram prog_; - /// Decision variables for QP step - std::vector x_; - std::vector u_; - std::vector lambda_; - - /// QP step constraints - std::vector dynamics_constraints_; - - // initial state constraint - std::vector> - constraints_; - // workspace and input limit constraints - std::vector> - user_constraints_; - /// QP step costs - std::vector target_cost_; - std::vector> costs_; - std::vector> input_costs_; - - // Solutions - mutable std::vector zfin_; - std::unique_ptr> x_sol_; - std::unique_ptr> lambda_sol_; - std::unique_ptr> u_sol_; - - std::unique_ptr> z_sol_; - std::unique_ptr> delta_sol_; - std::unique_ptr> w_sol_; -}; - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/c3_miqp.cc b/solvers/c3_miqp.cc deleted file mode 100644 index 48d89c6a3f..0000000000 --- a/solvers/c3_miqp.cc +++ /dev/null @@ -1,161 +0,0 @@ -#include "solvers/c3_miqp.h" - -namespace dairlib { -namespace solvers { - -using Eigen::MatrixXd; -using Eigen::VectorXd; -using std::vector; - -C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, - const vector& xdesired, const C3Options& options) - : C3Base(LCS, costs, xdesired, options), env_(true) { - // Create an environment - env_.set("LogToConsole", "0"); - env_.set("OutputFlag", "0"); - env_.set("Threads", "5"); - env_.start(); -} - -VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, - const VectorXd& delta_c, - const MatrixXd& E, const MatrixXd& F, - const MatrixXd& H, const VectorXd& c, - const int admm_iteration, - const int& warm_start_index) { - // set up linear term in cost - VectorXd cost_lin = -2 * delta_c.transpose() * U; - - //std::cout << "cost lin: " << cost_lin.transpose() << std::endl; - - // set up for constraints (Ex + F \lambda + Hu + c >= 0) - MatrixXd Mcons1(m_, n_ + m_ + k_); - Mcons1 << E, F, H; - - // set up for constraints (\lambda >= 0) - MatrixXd MM1 = MatrixXd::Zero(m_, n_); - MatrixXd MM2 = MatrixXd::Identity(m_, m_); - MatrixXd MM3 = MatrixXd::Zero(m_, k_); - MatrixXd Mcons2(m_, n_ + m_ + k_); - Mcons2 << MM1, MM2, MM3; - - GRBModel model = GRBModel(env_); - - GRBVar delta_k[n_ + m_ + k_]; - GRBVar binary[m_]; - - for (int i = 0; i < m_; i++) { - binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); - if (warm_start_index != -1) { - binary[i].set(GRB_DoubleAttr_Start, - warm_start_binary_[admm_iteration][warm_start_index](i)); - } - } - - for (int i = 0; i < n_ + m_ + k_; i++) { - delta_k[i] = model.addVar(-100.0, 100.0, 0.0, GRB_CONTINUOUS); - if (warm_start_index != -1) { - delta_k[i].set(GRB_DoubleAttr_Start, - warm_start_delta_[admm_iteration][warm_start_index](i)); - } - } - - GRBQuadExpr obj = 0; - - - for (int i = 0; i < n_ + m_ + k_; i++) { - obj.addTerm(cost_lin(i), delta_k[i]); - obj.addTerm(U(i, i), delta_k[i], delta_k[i]); - } - - model.setObjective(obj, GRB_MINIMIZE); - - int M = 100000; // big M variable - double coeff[n_ + m_ + k_]; - double coeff2[n_ + m_ + k_]; - - for (int i = 0; i < m_; i++) { - GRBLinExpr lambda_expr = 0; - - /// convert VectorXd to double - for (int j = 0; j < n_ + m_ + k_; j++) { - coeff[j] = Mcons2(i, j); - } - - lambda_expr.addTerms(coeff, delta_k, n_ + m_ + k_); - model.addConstr(lambda_expr >= 0); - model.addConstr(lambda_expr <= M * (1 - binary[i])); - - GRBLinExpr activation_expr = 0; - - /// convert VectorXd to double - for (int j = 0; j < n_ + m_ + k_; j++) { - coeff2[j] = Mcons1(i, j); - } - - activation_expr.addTerms(coeff2, delta_k, n_ + m_ + k_); - model.addConstr(activation_expr + c(i) >= 0); - model.addConstr(activation_expr + c(i) <= M * binary[i]); - - } - - // std::cout << "coeff: "; - // for (int i = 0; i < n_ + m_ + k_; i++) { - // std::cout << coeff[i] << ", "; - // } - // std::cout << "\n\ncoeff2: "; - // for (int i = 0; i < n_ + m_ + k_; i++) { - // std::cout << coeff2[i] << ", "; - // } - // std::cout << "\nc: " << std::endl; - // for (int i = 0; i < n_ + m_ + k_; i++) { - // std::cout << c(i) << ", "; - // } - // std::cout << "\n" << std::endl; - - // std::cout << "Before model.optimize" << std::endl; - try { - model.optimize(); - if (model.get(GRB_IntAttr_Status) == GRB_INFEASIBLE) { - model.computeIIS(); - model.write("model_IIS.ilp"); - } - int status = model.get(GRB_IntAttr_Status); - if (status == GRB_OPTIMAL || status == GRB_SUBOPTIMAL) { - - } else { - std::cerr << "Model did not solve to optimality. Status = " << status << std::endl; - } - } catch (GRBException& e) { - std::cerr << "Gurobi Exception: " << e.getMessage() - << " (error code " << e.getErrorCode() << ")" << std::endl; - } catch (std::exception& e) { - std::cerr << "Standard Exception: " << e.what() << std::endl; - } - - VectorXd delta_kc(n_ + m_ + k_); - VectorXd binaryc(m_); - for (int i = 0; i < n_ + m_ + k_; i++) { - delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); - } - for (int i = 0; i < m_; i++) { - binaryc(i) = binary[i].get(GRB_DoubleAttr_X); - } - - if (warm_start_index != -1) { - warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; - warm_start_binary_[admm_iteration][warm_start_index] = binaryc; - } - return delta_kc; -} - -std::vector C3MIQP::GetWarmStartDelta() const { - return warm_start_delta_[0]; -} - -std::vector C3MIQP::GetWarmStartBinary() const { - return warm_start_binary_[0]; -} - -} // namespace solvers -} // namespace dairlib \ No newline at end of file diff --git a/solvers/c3_miqp.h b/solvers/c3_miqp.h deleted file mode 100644 index fd1aa47db4..0000000000 --- a/solvers/c3_miqp.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include - -#include - -#include "gurobi_c++.h" -#include "solvers/base_c3.h" -#include "solvers/lcs.h" - -namespace dairlib { -namespace solvers { - -class C3MIQP final : public C3Base { - public: - /// Default constructor for time-varying LCS - C3MIQP(const LCS& LCS, const CostMatrices& costs, - const std::vector& xdesired, const C3Options& options); - - ~C3MIQP() override = default; - - /// Virtual projection method - Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U, - const Eigen::VectorXd& delta_c, - const Eigen::MatrixXd& E, - const Eigen::MatrixXd& F, - const Eigen::MatrixXd& H, - const Eigen::VectorXd& c, - const int admm_iteration, - const int& warm_start_index = -1) override; - std::vector GetWarmStartDelta() const; - std::vector GetWarmStartBinary() const; - - private: - GRBEnv env_; -}; - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/c3_options.h b/solvers/c3_options.h deleted file mode 100644 index b762fe5f37..0000000000 --- a/solvers/c3_options.h +++ /dev/null @@ -1,227 +0,0 @@ -#pragma once -#include - -#include "drake/common/yaml/yaml_read_archive.h" - -/* Ways of computing C3 costs after solving the MPC problem: - 0. kSimLCS: Simulate the LCS dynamics from the planned - inputs. - 1. kUseC3Plan: Use the C3 planned trajectory and inputs. - 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned - inputs only for the object; use the planned - EE trajectory. - 3. kSimImpedance: Try to emulate the real cost of the system - associated not only applying the planned - inputs, but also tracking the planned EE - trajectory with an impedance controller. - 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE - states are replaced with the plan from C3 at - the end. - 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the - object terms contribute to the final cost. -*/ -enum C3CostComputationType { - kSimLCS, - kUseC3Plan, - kSimLCSReplaceC3EEPlan, - kSimImpedance, - kSimImpedanceReplaceC3EEPlan, - kSimImpedanceObjectCostOnly, -}; - -struct C3Options { - // Hyperparameters - int admm_iter; // total number of ADMM iterations - float rho; // initial value of the rho parameter - float rho_scale; // scaling of rho parameter (/rho = rho_scale * /rho) - int num_threads; // 0 is dynamic, greater than 0 for a fixed count - int delta_option; // different options for delta update - std::string projection_type; - std::string contact_model; - bool warm_start; - bool use_predicted_x0; - bool end_on_qp_step; - double solve_time_filter_alpha; - double publish_frequency; - - std::vector u_horizontal_limits; - std::vector u_vertical_limits; - std::vector workspace_limits; - double workspace_margins; - - int N; - double gamma; - - double w_Q; - double w_R; - double w_G; - double w_U; - - std::vector q_vector; - std::vector r_vector; - - std::vector g_vector; - std::vector g_x; - std::vector g_gamma; - std::vector g_lambda_n; - std::vector g_lambda_t; - std::vector g_lambda; - std::vector g_eta_slack; - std::vector g_eta_n; - std::vector g_eta_t; - std::vector g_eta; - std::vector g_u; - - std::vector u_vector; - std::vector u_x; - std::vector u_gamma; - std::vector u_lambda_n; - std::vector u_lambda_t; - std::vector u_lambda; - std::vector u_eta_slack; - std::vector u_eta_n; - std::vector u_eta_t; - std::vector u_eta; - std::vector u_u; - - double qp_projection_alpha; - double qp_projection_scaling; - bool penalize_changes_in_u_across_solves; - std::vector mu; - double dt; - double solve_dt; // unused - int lcs_dt_resolution; - double dt_cost; - int num_friction_directions; - int num_contacts; - Eigen::MatrixXd Q; - Eigen::MatrixXd R; - Eigen::MatrixXd G; - Eigen::MatrixXd U; - - template - void Serialize(Archive* a) { - a->Visit(DRAKE_NVP(admm_iter)); - a->Visit(DRAKE_NVP(rho)); - a->Visit(DRAKE_NVP(rho_scale)); - a->Visit(DRAKE_NVP(num_threads)); - a->Visit(DRAKE_NVP(delta_option)); - a->Visit(DRAKE_NVP(contact_model)); - a->Visit(DRAKE_NVP(projection_type)); - if (projection_type == "QP") { - DRAKE_DEMAND(contact_model == "anitescu"); - } - a->Visit(DRAKE_NVP(warm_start)); - a->Visit(DRAKE_NVP(use_predicted_x0)); - a->Visit(DRAKE_NVP(end_on_qp_step)); - a->Visit(DRAKE_NVP(solve_time_filter_alpha)); - a->Visit(DRAKE_NVP(publish_frequency)); - - a->Visit(DRAKE_NVP(workspace_limits)); - a->Visit(DRAKE_NVP(u_horizontal_limits)); - a->Visit(DRAKE_NVP(u_vertical_limits)); - a->Visit(DRAKE_NVP(workspace_margins)); - - a->Visit(DRAKE_NVP(mu)); - a->Visit(DRAKE_NVP(dt)); - a->Visit(DRAKE_NVP(solve_dt)); - a->Visit(DRAKE_NVP(lcs_dt_resolution)); - a->Visit(DRAKE_NVP(dt_cost)); - a->Visit(DRAKE_NVP(num_friction_directions)); - a->Visit(DRAKE_NVP(num_contacts)); - - a->Visit(DRAKE_NVP(N)); - a->Visit(DRAKE_NVP(gamma)); - a->Visit(DRAKE_NVP(w_Q)); - a->Visit(DRAKE_NVP(w_R)); - a->Visit(DRAKE_NVP(w_G)); - a->Visit(DRAKE_NVP(w_U)); - a->Visit(DRAKE_NVP(q_vector)); - a->Visit(DRAKE_NVP(r_vector)); - a->Visit(DRAKE_NVP(g_x)); - a->Visit(DRAKE_NVP(g_gamma)); - a->Visit(DRAKE_NVP(g_lambda_n)); - a->Visit(DRAKE_NVP(g_lambda_t)); - a->Visit(DRAKE_NVP(g_lambda)); - a->Visit(DRAKE_NVP(g_u)); - a->Visit(DRAKE_NVP(u_x)); - a->Visit(DRAKE_NVP(u_gamma)); - a->Visit(DRAKE_NVP(u_lambda_n)); - a->Visit(DRAKE_NVP(u_lambda_t)); - a->Visit(DRAKE_NVP(u_lambda)); - a->Visit(DRAKE_NVP(u_u)); - - // Additional parameters for new C3 projection - a->Visit(DRAKE_NVP(g_eta_slack)); - a->Visit(DRAKE_NVP(g_eta_n)); - a->Visit(DRAKE_NVP(g_eta_t)); - a->Visit(DRAKE_NVP(g_eta)); - a->Visit(DRAKE_NVP(u_eta_slack)); - a->Visit(DRAKE_NVP(u_eta_n)); - a->Visit(DRAKE_NVP(u_eta_t)); - a->Visit(DRAKE_NVP(u_eta)); - - a->Visit(DRAKE_NVP(qp_projection_alpha)); - a->Visit(DRAKE_NVP(qp_projection_scaling)); - a->Visit(DRAKE_NVP(penalize_changes_in_u_across_solves)); - - g_vector = std::vector(); - g_vector.insert(g_vector.end(), g_x.begin(), g_x.end()); - if (contact_model == "stewart_and_trinkle") { - g_vector.insert(g_vector.end(), g_gamma.begin(), g_gamma.end()); - g_vector.insert(g_vector.end(), g_lambda_n.begin(), g_lambda_n.end()); - g_vector.insert(g_vector.end(), g_lambda_t.begin(), g_lambda_t.end()); - } else { - g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end()); - } - g_vector.insert(g_vector.end(), g_u.begin(), g_u.end()); - if (projection_type == "C3+") { - if (contact_model == "stewart_and_trinkle") { - g_vector.insert(g_vector.end(), g_eta_slack.begin(), g_eta_slack.end()); - g_vector.insert(g_vector.end(), g_eta_n.begin(), g_eta_n.end()); - g_vector.insert(g_vector.end(), g_eta_t.begin(), g_eta_t.end()); - } else { - g_vector.insert(g_vector.end(), g_eta.begin(), g_eta.end()); - } - } - - u_vector = std::vector(); - u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); - if (contact_model == "stewart_and_trinkle") { - u_vector.insert(u_vector.end(), u_gamma.begin(), u_gamma.end()); - u_vector.insert(u_vector.end(), u_lambda_n.begin(), u_lambda_n.end()); - u_vector.insert(u_vector.end(), u_lambda_t.begin(), u_lambda_t.end()); - } else { - u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end()); - } - u_vector.insert(u_vector.end(), u_u.begin(), u_u.end()); - if (projection_type == "C3+") { - if (contact_model == "stewart_and_trinkle") { - u_vector.insert(u_vector.end(), u_eta_slack.begin(), u_eta_slack.end()); - u_vector.insert(u_vector.end(), u_eta_n.begin(), u_eta_n.end()); - u_vector.insert(u_vector.end(), u_eta_t.begin(), u_eta_t.end()); - } else { - u_vector.insert(u_vector.end(), u_eta.begin(), u_eta.end()); - } - } - - Eigen::VectorXd q = Eigen::Map( - this->q_vector.data(), this->q_vector.size()); - Eigen::VectorXd r = Eigen::Map( - this->r_vector.data(), this->r_vector.size()); - Eigen::VectorXd g = Eigen::Map( - this->g_vector.data(), this->g_vector.size()); - Eigen::VectorXd u = Eigen::Map( - this->u_vector.data(), this->u_vector.size()); - - DRAKE_DEMAND(g_lambda.size() == num_contacts * num_friction_directions * 2); - DRAKE_DEMAND(u_lambda.size() == num_contacts * num_friction_directions * 2); - DRAKE_DEMAND(mu.size() == num_contacts); - DRAKE_DEMAND(g.size() == u.size()); - - Q = w_Q * q.asDiagonal(); - R = w_R * r.asDiagonal(); - G = w_G * g.asDiagonal(); - U = w_U * u.asDiagonal(); - } -}; \ No newline at end of file diff --git a/solvers/c3_output.cc b/solvers/c3_output.cc deleted file mode 100644 index 2f68be4b3d..0000000000 --- a/solvers/c3_output.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include "c3_output.h" - -using Eigen::VectorXd; -using Eigen::VectorXf; -using std::vector; - -namespace dairlib { - -C3Output::C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates) - : c3_solution_(c3_sol), c3_intermediates_(c3_intermediates) {} - -lcmt_c3_output C3Output::GenerateLcmObject(double time) const { - lcmt_c3_output c3_output; - lcmt_c3_solution c3_solution; - lcmt_c3_intermediates c3_intermediates; - c3_output.utime = static_cast(1e6 * time); - - c3_solution.num_points = c3_solution_.time_vector_.size(); - int knot_points = c3_solution.num_points; - c3_solution.num_state_variables = c3_solution_.x_sol_.rows(); - c3_solution.num_contact_variables = c3_solution_.lambda_sol_.rows(); - c3_solution.num_input_variables = c3_solution_.u_sol_.rows(); - c3_solution.time_vec.reserve(knot_points); - c3_solution.x_sol = vector>(c3_solution.num_state_variables, - vector(knot_points)); - c3_solution.lambda_sol = vector>( - c3_solution.num_contact_variables, vector(knot_points)); - c3_solution.u_sol = vector>(c3_solution.num_input_variables, - vector(knot_points)); - - c3_solution.time_vec = vector( - c3_solution_.time_vector_.data(), - c3_solution_.time_vector_.data() + c3_solution_.time_vector_.size()); - - c3_intermediates.num_total_variables = c3_intermediates_.delta_.rows(); - c3_intermediates.num_points = c3_solution.num_points; - c3_intermediates.time_vec.reserve(c3_intermediates.num_points); - c3_intermediates.z_sol = vector>( - c3_intermediates.num_total_variables, vector(knot_points)); - c3_intermediates.delta_sol = vector>( - c3_intermediates.num_total_variables, vector(knot_points)); - c3_intermediates.w_sol = vector>( - c3_intermediates.num_total_variables, vector(knot_points)); - c3_intermediates.time_vec = c3_solution.time_vec; - - for (int i = 0; i < c3_solution.num_state_variables; ++i) { - VectorXf temp_row = c3_solution_.x_sol_.row(i); - memcpy(c3_solution.x_sol[i].data(), temp_row.data(), - sizeof(float) * knot_points); - } - for (int i = 0; i < c3_solution.num_contact_variables; ++i) { - VectorXf temp_row = c3_solution_.lambda_sol_.row(i); - memcpy(c3_solution.lambda_sol[i].data(), temp_row.data(), - sizeof(float) * knot_points); - } - for (int i = 0; i < c3_solution.num_input_variables; ++i) { - VectorXf temp_row = c3_solution_.u_sol_.row(i); - memcpy(c3_solution.u_sol[i].data(), temp_row.data(), - sizeof(float) * knot_points); - } - for (int i = 0; i < c3_intermediates.num_total_variables; ++i) { - VectorXf temp_z_row = c3_intermediates_.z_.row(i); - VectorXf temp_delta_row = c3_intermediates_.delta_.row(i); - VectorXf temp_w_row = c3_intermediates_.w_.row(i); - memcpy(c3_intermediates.z_sol[i].data(), temp_z_row.data(), - sizeof(float) * knot_points); - memcpy(c3_intermediates.delta_sol[i].data(), temp_delta_row.data(), - sizeof(float) * knot_points); - memcpy(c3_intermediates.w_sol[i].data(), temp_w_row.data(), - sizeof(float) * knot_points); - } - - c3_output.c3_solution = c3_solution; - // Not assigning to reduce space -// c3_output.c3_intermediates = lcmt_c3_intermediates(); - c3_output.c3_intermediates = c3_intermediates; - - return c3_output; -} - -} // namespace dairlib diff --git a/solvers/c3_output.h b/solvers/c3_output.h deleted file mode 100644 index 6e4f574211..0000000000 --- a/solvers/c3_output.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -#include "dairlib/lcmt_c3_output.hpp" - -namespace dairlib { - -/// Used for outputting c3 solutions and intermediate variables for debugging purposes - -class C3Output { - public: - struct C3Solution { - C3Solution() = default; - - // Shape is (variable_vector_size, knot_points) - Eigen::VectorXf time_vector_; - Eigen::MatrixXf x_sol_; - Eigen::MatrixXf lambda_sol_; - Eigen::MatrixXf u_sol_; - }; - - struct C3Intermediates { - C3Intermediates() = default; - - // Shape is (variable_vector_size, knot_points) - Eigen::VectorXf time_vector_; - Eigen::MatrixXf z_; - Eigen::MatrixXf delta_; - Eigen::MatrixXf w_; - }; - - C3Output() = default; - C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates); - - explicit C3Output(const lcmt_c3_output& traj); - - virtual ~C3Output() = default; - - lcmt_c3_output GenerateLcmObject(double time) const; - - private: - C3Solution c3_solution_; - C3Intermediates c3_intermediates_; -}; - -} // namespace dairlib diff --git a/solvers/c3_plus.cc b/solvers/c3_plus.cc deleted file mode 100644 index c40bfec0a8..0000000000 --- a/solvers/c3_plus.cc +++ /dev/null @@ -1,167 +0,0 @@ -#include "solvers/c3_plus.h" - -#include - -#include - -#include "solvers/c3_options.h" -#include "solvers/lcs.h" - -namespace dairlib { -namespace solvers { - -using Eigen::MatrixXd; -using Eigen::VectorXd; -using std::vector; - -C3Plus::C3Plus(const LCS& LCS, const CostMatrices& costs, - const vector& xdesired, const C3Options& options) - : C3Base(LCS, costs, xdesired, options, - LCS.A_[0].cols() + 2 * LCS.D_[0].cols() + LCS.B_[0].cols()) { - // Initialize eta as optimization variables - eta_ = vector(); - eta_sol_ = std::make_unique>(); - for (int i = 0; i < N_; ++i) { - eta_sol_->push_back(Eigen::VectorXd::Zero(m_)); - eta_.push_back(prog_.NewContinuousVariables(m_, "eta" + std::to_string(i))); - } - - // Add eta equality constraints η = E * x + F * λ + H * u + c - MatrixXd EtaLinEq(m_, n_ + 2 * m_ + k_); - EtaLinEq.block(0, n_ + m_ + k_, m_, m_) = -1 * MatrixXd::Identity(m_, m_); - eta_constraints_.resize(N_); - for (int i = 0; i < N_; ++i) { - EtaLinEq.block(0, 0, m_, n_) = E_.at(i); - EtaLinEq.block(0, n_, m_, m_) = F_.at(i); - EtaLinEq.block(0, n_ + m_, m_, k_) = H_.at(i); - - eta_constraints_[i] = - prog_ - .AddLinearEqualityConstraint( - EtaLinEq, -c_.at(i), - {x_.at(i), lambda_.at(i), u_.at(i), eta_.at(i)}) - .evaluator() - .get(); - } - - // Disable parallelization for C3+ because of the overhead cost - use_parallelization_in_projection_ = false; -} - -void C3Plus::UpdateLCS(const LCS& lcs) { - C3Base::UpdateLCS(lcs); - - // Update eta equality constraints with new LCS - MatrixXd EtaLinEq(m_, n_ + 2 * m_ + k_); - EtaLinEq.block(0, n_ + m_ + k_, m_, m_) = -1 * MatrixXd::Identity(m_, m_); - for (int i = 0; i < N_; ++i) { - EtaLinEq.block(0, 0, m_, n_) = E_.at(i); - EtaLinEq.block(0, n_, m_, m_) = F_.at(i); - EtaLinEq.block(0, n_ + m_, m_, k_) = H_.at(i); - eta_constraints_[i]->UpdateCoefficients(EtaLinEq, -c_.at(i)); - } -} - -void C3Plus::AddAugmentedCostsQPStep(const std::vector& G, - const std::vector& WD, - const std::vector& delta, - bool is_final_solve) { - int large_coeff = 1000; - if (is_final_solve) { - std::vector last_qp_G = G; - for (int i = 0; i < N_; ++i) { - for (int j = 0; j < 4; ++j) { - if (delta.at(i)[n_ + j] == 0) { - last_qp_G.at(i).block(n_ + j, n_ + j, 1, 1) *= large_coeff; - } else { - last_qp_G.at(i).block(n_ + m_ + k_ + j, n_ + m_ + k_ + j, 1, 1) *= - large_coeff; - } - } - } - - for (int i = 0; i < N_; i++) { - costs_.push_back(prog_.AddQuadraticCost( - 2 * last_qp_G.at(i).block(n_, n_, m_, m_), - -2 * last_qp_G.at(i).block(n_, n_, m_, m_) * - delta.at(i).segment(n_, m_), - lambda_.at(i), 1)); - costs_.push_back(prog_.AddQuadraticCost( - 2 * last_qp_G.at(i).block(n_ + m_ + k_, n_ + m_ + k_, m_, m_), - -2 * last_qp_G.at(i).block(n_ + m_ + k_, n_ + m_ + k_, m_, m_) * - delta.at(i).segment(n_ + m_ + k_, m_), - eta_.at(i), 1)); - } - } else { - for (int i = 0; i < N_; i++) { - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(n_, n_, m_, m_), - -2 * G.at(i).block(n_, n_, m_, m_) * WD.at(i).segment(n_, m_), - lambda_.at(i), 1)); - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(n_ + m_ + k_, n_ + m_ + k_, m_, m_), - -2 * G.at(i).block(n_ + m_ + k_, n_ + m_ + k_, m_, m_) * - WD.at(i).segment(n_ + m_ + k_, m_), - eta_.at(i), 1)); - } - } -} - -void C3Plus::ExtractQPSolution( - const drake::solvers::MathematicalProgramResult& result, int admm_iteration, - bool is_final_solve) { - C3Base::ExtractQPSolution(result, admm_iteration, is_final_solve); - for (int i = 0; i < N_; i++) { - if (is_final_solve) { - eta_sol_->at(i) = result.GetSolution(eta_[i]); - } - z_sol_->at(i).segment(n_ + m_ + k_, m_) = result.GetSolution(eta_[i]); - } -} - -void C3Plus::UpdateWarmStarts( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration) { - C3Base::UpdateWarmStarts(result, admm_iteration); -} - -VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, - const VectorXd& delta_c, - const MatrixXd& E, const MatrixXd& F, - const MatrixXd& H, const VectorXd& c, - const int admm_iteration, - const int& warm_start_index) { - VectorXd delta_proj = delta_c; - - // Extract the weight vectors for lambda and eta from the diagonal of the cost - // matrix U. - VectorXd w_eta_vec = U.block(n_ + m_ + k_, n_ + m_ + k_, m_, m_).diagonal(); - VectorXd w_lambda_vec = U.block(n_, n_, m_, m_).diagonal(); - - // Throw an error if any weights are negative. - if (w_eta_vec.minCoeff() < 0 || w_lambda_vec.minCoeff() < 0) { - throw std::runtime_error( - "Negative weights in the cost matrix U are not allowed."); - } - - VectorXd lambda_c = delta_c.segment(n_, m_); - VectorXd eta_c = delta_c.segment(n_ + m_ + k_, m_); - - // Set the smaller of lambda and eta to zero - Eigen::Array eta_larger = - eta_c.array() * w_eta_vec.array().sqrt() > - lambda_c.array() * w_lambda_vec.array().sqrt(); - - delta_proj.segment(n_, m_) = eta_larger.select(VectorXd::Zero(m_), lambda_c); - delta_proj.segment(n_ + m_ + k_, m_) = - eta_larger.select(eta_c, VectorXd::Zero(m_)); - - // Clip lambda and eta at 0 - delta_proj.segment(n_, m_) = delta_proj.segment(n_, m_).cwiseMax(0); - delta_proj.segment(n_ + m_ + k_, m_) = - delta_proj.segment(n_ + m_ + k_, m_).cwiseMax(0); - - return delta_proj; -} -} // namespace solvers -} // namespace dairlib diff --git a/solvers/c3_plus.h b/solvers/c3_plus.h deleted file mode 100644 index e5e2556bde..0000000000 --- a/solvers/c3_plus.h +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include - -#include - -#include "solvers/base_c3.h" -#include "solvers/c3_options.h" -#include "solvers/lcs.h" - -namespace dairlib { -namespace solvers { - -// C3+ is a variant of C3 that introduces a new slack variable, η, and a new -// equality constraint, η = E * x + F * λ + H * u + c, to the QP step. -// This allows us to solve the projection step in a more efficient way (without -// solving any optimization problem). -// The z and delta vector now have the following structure [x, λ, u, η]. -// -// In C3+ QP step, we solve the following problem: -// -// min_{x, u, λ, η} g_x ||x - x_des||² + g_u ||u||² + -// g_λ ||λ - λ₀||² + g_η ||η - η₀||² -// s.t. x_next = A * x + B * u + D * λ + d -// η = E * x + F * λ + H * u + c -// u_min ≤ u ≤ u_max -// x_min ≤ x ≤ x_max -// -// In C3+ projection step, we aim to solve the following problem -// -// min_{λ, η} w_λ ||λ - λ₀||² + w_η ||η - η₀||² -// s.t. 0 ≤ λ ⊥ η ≥ 0 -// -// where λ₀ and η₀ are the values of λ and η obtained from the QP step, -// respectively. The solution to this problem is the projection of (λ₀, η₀) -// onto the feasible set defined by the complementarity condition (i.e., λᵢ ηᵢ -// = 0 for all i, with λ ≥ 0 and η ≥ 0). -// -// To get the solution, we can simply do the following steps: -// 1. If η₀ > sqrt(w_λ/w_η) * λ₀, then λ = 0, else η = 0 -// 2. [λ, η] = max(0, [λ, η]) -class C3Plus final : public C3Base { - public: - C3Plus(const LCS& LCS, const CostMatrices& costs, - const std::vector& xdesired, - const C3Options& options); - ~C3Plus() override = default; - - Eigen::VectorXd SolveSingleProjection( - const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, - const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, - const Eigen::MatrixXd& H, const Eigen::VectorXd& c, - const int admm_iteration, const int& warm_start_index = -1) override; - - private: - void UpdateLCS(const LCS& lcs) override; - void AddAugmentedCostsQPStep(const std::vector& G, - const std::vector& WD, - const std::vector& delta, - bool is_final_solve) override; - void ExtractQPSolution( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve) override; - void UpdateWarmStarts(const drake::solvers::MathematicalProgramResult& result, - int admm_iteration) override; - std::vector eta_; - std::unique_ptr> eta_sol_; - - // Store the following constraint η = E * x + F * λ + H * u + c - std::vector eta_constraints_; -}; - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/c3_qp.cc b/solvers/c3_qp.cc deleted file mode 100644 index 3ea90b0fc3..0000000000 --- a/solvers/c3_qp.cc +++ /dev/null @@ -1,106 +0,0 @@ -#include "solvers/c3_qp.h" - -#include - -#include - -#include "solvers/c3_options.h" -#include "solvers/lcs.h" - -#include "drake/solvers/mathematical_program.h" -#include "drake/solvers/osqp_solver.h" -#include "drake/solvers/solve.h" - -namespace dairlib { -namespace solvers { - -using Eigen::MatrixXd; -using Eigen::VectorXd; -using std::vector; - -using drake::solvers::MathematicalProgram; -using drake::solvers::MathematicalProgramResult; -using drake::solvers::SolutionResult; -using drake::solvers::SolverOptions; - -using drake::solvers::OsqpSolver; -using drake::solvers::OsqpSolverDetails; -using drake::solvers::Solve; - -C3QP::C3QP(const LCS& LCS, const CostMatrices& costs, - const vector& xdesired, const C3Options& options) - : C3Base(LCS, costs, xdesired, options) {} - -VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, - const MatrixXd& E, const MatrixXd& F, - const MatrixXd& H, const VectorXd& c, - const int admm_iteration, - const int& warm_start_index) { - drake::solvers::MathematicalProgram prog; - drake::solvers::SolverOptions solver_options; - drake::solvers::OsqpSolver osqp_; - - auto xn_ = prog.NewContinuousVariables(n_, "x"); - auto ln_ = prog.NewContinuousVariables(m_, "lambda"); - auto un_ = prog.NewContinuousVariables(k_, "u"); - - // As alpha -> 0, the complimentarity condition is enforced more strictly. - double alpha = options_.qp_projection_alpha; - double scaling = options_.qp_projection_scaling; - - MatrixXd EFH = MatrixXd::Zero(m_, n_ + m_ + k_); - EFH.block(0, 0, m_, n_) = E / scaling; - EFH.block(0, n_, m_, m_) = F / scaling; - EFH.block(0, n_ + m_, m_, k_) = H / scaling; - - prog.AddLinearConstraint( - EFH, -c / scaling, - Eigen::VectorXd::Constant(m_, std::numeric_limits::infinity()), - {xn_, ln_, un_}); - - prog.AddLinearConstraint( - MatrixXd::Identity(m_, m_), VectorXd::Zero(m_), - Eigen::VectorXd::Constant(m_, std::numeric_limits::infinity()), - ln_); - - MatrixXd New_U = U; - New_U.block(n_, n_, m_, m_) = alpha * F; - - VectorXd cost_linear = -delta_c.transpose() * New_U; - - prog.AddQuadraticCost(New_U, cost_linear, {xn_, ln_, un_}, 1); - - prog.AddQuadraticCost((1 - alpha) * F, VectorXd::Zero(m_), ln_, 1); - - solver_options.SetOption(OsqpSolver::id(), "max_iter", 500); - solver_options.SetOption(OsqpSolver::id(), "verbose", 0); - solver_options.SetOption(OsqpSolver::id(), "polish", 1); - solver_options.SetOption(OsqpSolver::id(), "polish_refine_iter", 1); - solver_options.SetOption(OsqpSolver::id(), "rho", 1e-4); - solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1); - prog.SetSolverOptions(solver_options); - - MathematicalProgramResult result = osqp_.Solve(prog); - - VectorXd xSol = result.GetSolution(xn_); - VectorXd lamSol = result.GetSolution(ln_); - VectorXd uSol = result.GetSolution(un_); - - VectorXd delta_kc = VectorXd::Zero(n_ + m_ + k_); - delta_kc.segment(0, n_) = xSol; - delta_kc.segment(n_, m_) = lamSol; - delta_kc.segment(n_ + m_, k_) = uSol; - - return delta_kc; -} - -std::vector C3QP::GetWarmStartDelta() const { - return warm_start_delta_[0]; -} - -std::vector C3QP::GetWarmStartBinary() const { - return warm_start_binary_[0]; -} - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/c3_qp.h b/solvers/c3_qp.h deleted file mode 100644 index dc47bf1dce..0000000000 --- a/solvers/c3_qp.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include - -#include - -#include "solvers/base_c3.h" -#include "solvers/lcs.h" - -#include "drake/solvers/mathematical_program.h" -#include "drake/solvers/moby_lcp_solver.h" -#include "drake/solvers/osqp_solver.h" -#include "drake/solvers/solve.h" - -#include "solvers/c3_options.h" -#include "solvers/fast_osqp_solver.h" - - -namespace dairlib { -namespace solvers { - -class C3QP final : public C3Base { - public: - /// Default constructor for time-varying LCS - C3QP(const LCS& LCS, const CostMatrices& costs, - const std::vector& xdesired, - const C3Options& options); - - ~C3QP() override = default; - - /// Virtual projection method - Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U, - const Eigen::VectorXd& delta_c, - const Eigen::MatrixXd& E, - const Eigen::MatrixXd& F, - const Eigen::MatrixXd& H, - const Eigen::VectorXd& c, - const int admm_iteration, - const int& warm_start_index = -1) override; - std::vector GetWarmStartDelta() const; - std::vector GetWarmStartBinary() const; - -}; - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/lcs.cc b/solvers/lcs.cc deleted file mode 100644 index c6402a0554..0000000000 --- a/solvers/lcs.cc +++ /dev/null @@ -1,124 +0,0 @@ -#include "solvers/lcs.h" - -#include - -#include "drake/solvers/mathematical_program.h" -#include "drake/solvers/moby_lcp_solver.h" -using Eigen::MatrixXd; -using Eigen::VectorXd; -using std::vector; - -namespace dairlib { -namespace solvers { - -LCS::LCS(const vector& A, const vector& B, - const vector& D, const vector& d, - const vector& E, const vector& F, - const vector& H, const vector& c, double dt) - : A_(A), - B_(B), - D_(D), - d_(d), - E_(E), - F_(F), - H_(H), - c_(c), - N_(A_.size()), - dt_(dt), - n_(A_[0].rows()), - m_(D_[0].cols()), - k_(H_[0].cols()) {} - -LCS::LCS(const MatrixXd& A, const MatrixXd& B, const MatrixXd& D, - const VectorXd& d, const MatrixXd& E, const MatrixXd& F, - const MatrixXd& H, const VectorXd& c, const int& N, double dt) - : LCS(vector(N, A), vector(N, B), - vector(N, D), vector(N, d), - vector(N, E), vector(N, F), - vector(N, H), vector(N, c), dt) { - } - -LCS::LCS(const LCS& lcs) - : N_(lcs.N_), dt_(lcs.dt_), n_(lcs.n_), m_(lcs.m_), k_(lcs.k_) { - A_.resize(N_); - B_.resize(N_); - D_.resize(N_); - d_.resize(N_); - E_.resize(N_); - F_.resize(N_); - H_.resize(N_); - c_.resize(N_); - for (int i = 0; i < lcs.N_; ++i) { - A_.at(i) = lcs.A_.at(i); - B_.at(i) = lcs.B_.at(i); - D_.at(i) = lcs.D_.at(i); - d_.at(i) = lcs.d_.at(i); - E_.at(i) = lcs.E_.at(i); - F_.at(i) = lcs.F_.at(i); - H_.at(i) = lcs.H_.at(i); - c_.at(i) = lcs.c_.at(i); - } - W_x_ = lcs.W_x_; - W_l_ = lcs.W_l_; - W_u_ = lcs.W_u_; - w_ = lcs.w_; - has_tangent_linearization_ = lcs.has_tangent_linearization_; -} - -LCS& LCS::operator=(const LCS& lcs) { - N_ = lcs.N_; - dt_ = lcs.dt_; - n_ = lcs.n_; - m_ = lcs.m_; - k_ = lcs.k_; - A_.resize(N_); - B_.resize(N_); - D_.resize(N_); - d_.resize(N_); - E_.resize(N_); - F_.resize(N_); - H_.resize(N_); - c_.resize(N_); - for (int i = 0; i < lcs.N_; ++i) { - A_.at(i) = lcs.A_.at(i); - B_.at(i) = lcs.B_.at(i); - D_.at(i) = lcs.D_.at(i); - d_.at(i) = lcs.d_.at(i); - E_.at(i) = lcs.E_.at(i); - F_.at(i) = lcs.F_.at(i); - H_.at(i) = lcs.H_.at(i); - c_.at(i) = lcs.c_.at(i); - } - W_x_ = lcs.W_x_; - W_l_ = lcs.W_l_; - W_u_ = lcs.W_u_; - w_ = lcs.w_; - has_tangent_linearization_ = lcs.has_tangent_linearization_; - return *this; -} - -const VectorXd LCS::Simulate(const VectorXd& x_init, const VectorXd& input, - bool verbose) { - VectorXd x_final; - // calculate force - drake::solvers::MobyLCPSolver LCPSolver; - VectorXd force; - - auto flag = LCPSolver.SolveLcpFastRegularized( - F_[0], E_[0] * x_init + c_[0] + H_[0] * input, &force, -8, 1, 1); - - if (flag == 0) { - std::cout << "LCP failed: returning x_init" << std::endl; - return x_init; - } - - // update - x_final = A_[0] * x_init + B_[0] * input + D_[0] * force + d_[0]; - if (verbose) { - std::cout<<"\tLCP simulated force is "< -#include - -#include - -#include "drake/common/sorted_pair.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace dairlib { -namespace solvers { -class LCS { - public: - /// Constructor for time-varying LCS - /// @param A, B, D, d Dynamics constraints x_{k+1} = A_k x_k + B_k u_k + D_k - /// \lambda_k + d_k - /// @param E, F, H, c Complementarity constraints 0 <= \lambda_k \perp E_k - /// x_k + F_k \lambda_k + H_k u_k + c_k - LCS(const std::vector& A, - const std::vector& B, - const std::vector& D, - const std::vector& d, - const std::vector& E, - const std::vector& F, - const std::vector& H, - const std::vector& c, double dt); - - /// Constructor for time-invariant LCS - LCS(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, - const Eigen::MatrixXd& D, const Eigen::VectorXd& d, - const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, - const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int& N, - double dt); - - LCS(const LCS& other); - LCS& operator=(const LCS&); - LCS(LCS&&) = default; - LCS& operator=(LCS&&) = default; - - void SetTangentGapLinearization(const Eigen::MatrixXd& W_x, - const Eigen::MatrixXd& W_l, - const Eigen::MatrixXd& W_u, - const Eigen::VectorXd& w) { - W_x_ = W_x; - W_l_ = W_l; - W_u_ = W_u; - w_ = w; - has_tangent_linearization_ = true; - } - - /// Simulate the system for one-step - /// @param x_init Initial x value - /// @param input Input value - const Eigen::VectorXd Simulate(const Eigen::VectorXd& x_init, - const Eigen::VectorXd& input, - bool verbose = false); - - public: - std::vector A_; - std::vector B_; - std::vector D_; - std::vector d_; - std::vector E_; - std::vector F_; - std::vector H_; - std::vector c_; - Eigen::MatrixXd W_x_; - Eigen::MatrixXd W_l_; - Eigen::MatrixXd W_u_; - Eigen::VectorXd w_; - bool has_tangent_linearization_ = false; - Eigen::MatrixXd J_c_; - int N_; - double dt_; - - int n_; - int m_; - int k_; -}; - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/lcs_factory.cc b/solvers/lcs_factory.cc deleted file mode 100644 index 9902c812e2..0000000000 --- a/solvers/lcs_factory.cc +++ /dev/null @@ -1,558 +0,0 @@ -#include "solvers/lcs_factory.h" - -#include - -#include "multibody/geom_geom_collider.h" -#include "multibody/kinematic/kinematic_evaluator_set.h" - -#include "drake/math/autodiff_gradient.h" -#include "drake/solvers/moby_lcp_solver.h" - -namespace dairlib { -namespace solvers { - -using std::set; -using std::vector; - -using drake::AutoDiffVecXd; -using drake::AutoDiffXd; -using drake::MatrixX; -using drake::SortedPair; -using drake::VectorX; -using drake::geometry::GeometryId; -using drake::math::ExtractGradient; -using drake::math::ExtractValue; -using drake::multibody::MultibodyPlant; -using drake::systems::Context; - -using Eigen::MatrixXd; -using Eigen::VectorXd; - -LCS LCSFactory::LinearizePlantToLCS( - const MultibodyPlant& plant, const Context& context, - const MultibodyPlant& plant_ad, - const Context& context_ad, - const vector>& contact_geoms, - const std::vector& mu, double dt, - int N, int n_lambda_with_tangential, - const std::vector& num_friction_directions_per_contact, - const std::vector& starting_index_per_contact_in_lambda_t_vector, - ContactModel contact_model) { - int n_x = plant_ad.num_positions() + plant_ad.num_velocities(); - int n_u = plant_ad.num_actuators(); - - int n_contacts = contact_geoms.size(); - - DRAKE_DEMAND(plant_ad.num_velocities() == plant.num_velocities()); - DRAKE_DEMAND(plant_ad.num_positions() == plant.num_positions()); - DRAKE_DEMAND(mu.size() == n_contacts); - int n_v = plant.num_velocities(); - int n_q = plant.num_positions(); - - AutoDiffVecXd C(n_v); - plant_ad.CalcBiasTerm(context_ad, &C); - VectorXd u_dyn = plant.get_actuation_input_port().Eval(context); - auto B_dyn_ad = plant_ad.MakeActuationMatrix(); - AutoDiffVecXd Bu = - B_dyn_ad * plant_ad.get_actuation_input_port().Eval(context_ad); - - AutoDiffVecXd tau_g = plant_ad.CalcGravityGeneralizedForces(context_ad); - - drake::multibody::MultibodyForces f_app(plant_ad); - plant_ad.CalcForceElementsContribution(context_ad, &f_app); - - MatrixX M(n_v, n_v); - plant_ad.CalcMassMatrix(context_ad, &M); - - // If this ldlt is slow, there are alternate formulations which avoid it - AutoDiffVecXd vdot_no_contact = - M.ldlt().solve(tau_g + Bu + f_app.generalized_forces() - C); - // Constant term in dynamics, d_vv = d + A x_0 + B u_0 - VectorXd d_vv = ExtractValue(vdot_no_contact); - // Derivatives w.r.t. x and u, AB - MatrixXd AB_v = ExtractGradient(vdot_no_contact); - VectorXd x_dvv(n_q + n_v + n_u); - x_dvv << plant.GetPositions(context), plant.GetVelocities(context), u_dyn; - VectorXd x_dvvcomp = AB_v * x_dvv; - VectorXd d_v = d_vv - x_dvvcomp; - - /////////// - AutoDiffVecXd x_ad = plant_ad.GetPositionsAndVelocities(context_ad); - AutoDiffVecXd qdot_no_contact(n_q); - AutoDiffVecXd vel_ad = x_ad.tail(n_v); - - plant_ad.MapVelocityToQDot(context_ad, vel_ad, &qdot_no_contact); - MatrixXd AB_q = ExtractGradient(qdot_no_contact); - MatrixXd d_q = ExtractValue(qdot_no_contact); - Eigen::SparseMatrix Nqt; - Nqt = plant.MakeVelocityToQDotMap(context); - MatrixXd qdotNv = MatrixXd(Nqt); - - Eigen::SparseMatrix NqI; - NqI = plant.MakeQDotToVelocityMap(context); - MatrixXd vNqdot = MatrixXd(NqI); - - VectorXd phi(n_contacts); - MatrixXd J_n(n_contacts, n_v); - MatrixXd J_t(n_lambda_with_tangential, n_v); - - // Perpendicular to the plane where c3 works - Eigen::Vector3d planar_normal(0, 0, 1); - for (int i = 0; i < n_contacts; i++) { - multibody::GeomGeomCollider collider( - plant, - contact_geoms[i]); - - auto [phi_i, J_i] = - (num_friction_directions_per_contact[i] == 1) ? collider.EvalPlanar(context, planar_normal) : - collider.EvalPolytope(context, num_friction_directions_per_contact[i]); - - phi(i) = phi_i; - J_n.row(i) = J_i.row(0); - J_t.block(starting_index_per_contact_in_lambda_t_vector[i], - 0, 2 * num_friction_directions_per_contact[i], n_v) = - J_i.block(1, 0, 2 * num_friction_directions_per_contact[i], n_v); - - // J_i is 3 x n_v - // row (0) is contact normal - // rows (1-num_friction directions) are the contact tangents - } - - auto M_ldlt = ExtractValue(M).ldlt(); - MatrixXd MinvJ_n_T = M_ldlt.solve(J_n.transpose()); - MatrixXd MinvJ_t_T = M_ldlt.solve(J_t.transpose()); - - MatrixXd A = MatrixXd::Zero(n_x, n_x); - MatrixXd B = MatrixXd::Zero(n_x, n_u); - VectorXd d = VectorXd::Zero(n_x); - - MatrixXd AB_v_q = AB_v.block(0, 0, n_v, n_q); - MatrixXd AB_v_v = AB_v.block(0, n_q, n_v, n_v); - MatrixXd AB_v_u = AB_v.block(0, n_x, n_v, n_u); - MatrixXd M_double = MatrixXd::Zero(n_v, n_v); - plant.CalcMassMatrix(context, &M_double); - - A.block(0, 0, n_q, n_q) = - MatrixXd::Identity(n_q, n_q) + dt * dt * qdotNv * AB_v_q; - A.block(0, n_q, n_q, n_v) = dt * qdotNv + dt * dt * qdotNv * AB_v_v; - A.block(n_q, 0, n_v, n_q) = dt * AB_v_q; - A.block(n_q, n_q, n_v, n_v) = dt * AB_v_v + MatrixXd::Identity(n_v, n_v); - - B.block(0, 0, n_q, n_u) = dt * dt * qdotNv * AB_v_u; - B.block(n_q, 0, n_v, n_u) = dt * AB_v_u; - - d.head(n_q) = dt * dt * qdotNv * d_v; - d.tail(n_v) = dt * d_v; - - MatrixXd E_t = - MatrixXd::Zero(n_contacts, n_lambda_with_tangential); - for (int i = 0; i < n_contacts; i++) { - E_t.block(i, starting_index_per_contact_in_lambda_t_vector[i], - 1, 2 * num_friction_directions_per_contact[i]) = - MatrixXd::Ones(1, 2 * num_friction_directions_per_contact[i]); - - } - - int n_lambda = 0; - if (contact_model == ContactModel::kStewartAndTrinkle) { - n_lambda = 2 * n_contacts + n_lambda_with_tangential; - } else { - n_lambda = n_lambda_with_tangential; - } - - // Matrices with contact variables - MatrixXd D = MatrixXd::Zero(n_x, n_lambda); - MatrixXd E = MatrixXd::Zero(n_lambda, n_x); - MatrixXd F = MatrixXd::Zero(n_lambda, n_lambda); - MatrixXd H = MatrixXd::Zero(n_lambda, n_u); - VectorXd c = VectorXd::Zero(n_lambda); - - MatrixXd W_x = MatrixXd::Zero(n_lambda, n_x); - MatrixXd W_l = MatrixXd::Zero(n_lambda, n_lambda); - MatrixXd W_u = MatrixXd::Zero(n_lambda, n_u); - MatrixXd w = VectorXd::Zero(n_lambda); - - if (contact_model == ContactModel::kStewartAndTrinkle) { - D.block(0, 2 * n_contacts, n_q, n_lambda_with_tangential) = - dt * dt * qdotNv * MinvJ_t_T; - D.block(n_q, 2 * n_contacts, n_v, - n_lambda_with_tangential) = dt * MinvJ_t_T; - D.block(0, n_contacts, n_q, n_contacts) = dt * dt * qdotNv * MinvJ_n_T; - - D.block(n_q, n_contacts, n_v, n_contacts) = dt * MinvJ_n_T; - // Complementarity condition for gamma: mu lambda^n - E.block(n_contacts, 0, n_contacts, n_q) = - dt * dt * J_n * AB_v_q + J_n * vNqdot; - E.block(2 * n_contacts, 0, n_lambda_with_tangential, n_q) = - dt * J_t * AB_v_q; - E.block(n_contacts, n_q, n_contacts, n_v) = - dt * J_n + dt * dt * J_n * AB_v_v; - E.block(2 * n_contacts, n_q, n_lambda_with_tangential, - n_v) = J_t + dt * J_t * AB_v_v; - - VectorXd mu_vec = Eigen::Map( - mu.data(), mu.size()); - // Complementarity condition for gamma: mu lambda^n - F.block(0, n_contacts, n_contacts, n_contacts) = mu_vec.asDiagonal(); - - // Complementarity condition for gamma: lambda^t - F.block(0, 2 * n_contacts, n_contacts, - n_lambda_with_tangential) = -E_t; - - // Complementarity condition for lambda_n: dt J_n (lambda^n component of - // v_{k+1}) - F.block(n_contacts, n_contacts, n_contacts, n_contacts) = - dt * dt * J_n * MinvJ_n_T; - // Complementarity condition for lambda_n: dt J_n (lambda^t component of - // v_{k+1}) - F.block(n_contacts, 2 * n_contacts, n_contacts, - n_lambda_with_tangential) = - dt * dt * J_n * MinvJ_t_T; - // Complementarity condition for lambda_t: dt J_t (gamma component of - // v_{k+1}) - F.block(2 * n_contacts, 0, n_lambda_with_tangential, - n_contacts) = E_t.transpose(); - // Complementarity condition for lambda_t: dt J_t (lambda^n component of - // v_{k+1}) - F.block(2 * n_contacts, n_contacts, - n_lambda_with_tangential, n_contacts) = - dt * J_t * MinvJ_n_T; - // Complementarity condition for lambda_t: dt J_t (lambda^t component of - // v_{k+1}) - F.block(2 * n_contacts, 2 * n_contacts, - n_lambda_with_tangential, - n_lambda_with_tangential) = dt * J_t * MinvJ_t_T; - - H.block(n_contacts, 0, n_contacts, n_u) = dt * dt * J_n * AB_v_u; - H.block(2 * n_contacts, 0, n_lambda_with_tangential, n_u) = - dt * J_t * AB_v_u; - - c.segment(n_contacts, n_contacts) = - phi + dt * dt * J_n * d_v - J_n * vNqdot * plant.GetPositions(context); - c.segment(2 * n_contacts, n_lambda_with_tangential) = - J_t * dt * d_v; - } else if (contact_model == ContactModel::kAnitescu) { - VectorXd mu_vec = Eigen::Map( - mu.data(), mu.size()); - VectorXd anitescu_mu_vec = VectorXd::Zero(n_lambda); - for (int i = 0; i < mu_vec.rows(); i++) { - anitescu_mu_vec.segment(starting_index_per_contact_in_lambda_t_vector[i], - 2 * num_friction_directions_per_contact[i]).setConstant(mu[i]); - } - - MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); - // Constructing friction bases - MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; - - MatrixXd MinvJ_c_T = M_ldlt.solve(J_c.transpose()); - - D.block(0, 0, n_q, n_lambda) = dt * dt * qdotNv * MinvJ_c_T; - D.block(n_q, 0, n_v, n_lambda) = dt * MinvJ_c_T; - - // q component of complementarity constraint - E.block(0, 0, n_lambda, n_q) = - dt * J_c * AB_v_q + E_t.transpose() * J_n * vNqdot / dt; - E.block(0, n_q, n_lambda, n_v) = J_c + dt * J_c * AB_v_v; - - // lambda component of complementarity constraint - F = dt * J_c * MinvJ_c_T; - - // u component of complementarity constraint - H = dt * J_c * AB_v_u; - // constant component of complementarity constraint - c = E_t.transpose() * phi / dt + dt * J_c * d_v - - E_t.transpose() * J_n * vNqdot * plant.GetPositions(context) / dt; - - // Anitescu model needs an explicit formulation for the tangential - // components in order to appropriately activate the robust constraint - // (TODO): yangwill do another pass to verify this formulation - W_x.block(0, 0, n_lambda, n_q) = J_t * AB_v_q; - W_x.block(0, n_q, n_lambda, n_v) = J_t + J_t * AB_v_v; - W_l = J_t * (MinvJ_c_T); - W_u = J_t * (AB_v_u); - w = J_t * (d_v); - } - LCS system(A, B, D, d, E, F, H, c, N, dt); - system.SetTangentGapLinearization(W_x, W_l, W_u, w); - return system; -} - -std::pair> -LCSFactory::ComputeContactJacobian( - const drake::multibody::MultibodyPlant& plant, - const drake::systems::Context& context, - const std::vector>& - contact_geoms, - const std::vector& mu, int n_lambda_with_tangential, - const std::vector& num_friction_directions_per_contact, - const std::vector& starting_index_per_contact_in_lambda_t_vector, - dairlib::solvers::ContactModel contact_model) { - - int n_contacts = contact_geoms.size(); - - int n_v = plant.num_velocities(); - - VectorXd phi(n_contacts); - MatrixXd J_n(n_contacts, n_v); - MatrixXd J_t(n_lambda_with_tangential, n_v); - std::vector contact_points; - - for (int i = 0; i < n_contacts; i++) { - multibody::GeomGeomCollider collider(plant, contact_geoms[i]); - Eigen::Vector3d planar_normal(0, 0, 1); - auto [phi_i, J_i] = - (num_friction_directions_per_contact[i] == 1) ? collider.EvalPlanar(context, planar_normal): - collider.EvalPolytope(context, num_friction_directions_per_contact[i]); - auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context); - // TODO(yangwill): think about if we want to push back both witness points - contact_points.push_back(p_WCa); - phi(i) = phi_i; - J_n.row(i) = J_i.row(0); - J_t.block(starting_index_per_contact_in_lambda_t_vector[i], - 0, 2 * num_friction_directions_per_contact[i], n_v) = - J_i.block(1, 0, 2 * num_friction_directions_per_contact[i], n_v); - // J_i is 3 x n_v - // row (0) is contact normal - // rows (1-num_friction directions) are the contact tangents - } - - if (contact_model == ContactModel::kStewartAndTrinkle) { - MatrixXd J_c = MatrixXd::Zero( - n_contacts + n_lambda_with_tangential, n_v); - J_c << J_n, J_t; - return std::make_pair(J_c, contact_points); - } else if (contact_model == ContactModel::kAnitescu) { - MatrixXd E_t = - MatrixXd::Zero(n_contacts, n_lambda_with_tangential); - - for (int i = 0; i < n_contacts; i++) { - E_t.block(i, starting_index_per_contact_in_lambda_t_vector[i], - 1, 2 * num_friction_directions_per_contact[i]) = - MatrixXd::Ones(1, 2 * num_friction_directions_per_contact[i]); - } - - VectorXd mu_vec = Eigen::Map( - mu.data(), mu.size()); - VectorXd anitescu_mu_vec = VectorXd::Zero(n_lambda_with_tangential); - - for (int i = 0; i < mu_vec.rows(); i++) { - anitescu_mu_vec.segment(starting_index_per_contact_in_lambda_t_vector[i], - 2 * num_friction_directions_per_contact[i]).setConstant(mu[i]); - } - - MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); - MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; - return std::make_pair(J_c, contact_points); - } else { - std::cerr << ("Unknown projection type") << std::endl; - DRAKE_THROW_UNLESS(false); - } -} - -LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, - set inactive_lambda_inds) { - vector remaining_inds; - - // Assumes constant number of contacts per index - int n_lambda = other.F_[0].rows(); - - // Need to solve for lambda_active in terms of remaining elements - // Build temporary [F1, F2] by eliminating rows for inactive - for (int i = 0; i < n_lambda; i++) { - // active/inactive must be exclusive - DRAKE_ASSERT(!active_lambda_inds.count(i) || - !inactive_lambda_inds.count(i)); - - // In C++20, could use contains instead of count - if (!active_lambda_inds.count(i) && !inactive_lambda_inds.count(i)) { - remaining_inds.push_back(i); - } - } - - int n_remaining = remaining_inds.size(); - int n_active = active_lambda_inds.size(); - - vector A, B, D, E, F, H; - vector d, c; - - // Build selection matrices: - // S_a selects active indices - // S_r selects remaining indices - - MatrixXd S_a = MatrixXd::Zero(n_active, n_lambda); - MatrixXd S_r = MatrixXd::Zero(n_remaining, n_lambda); - - for (int i = 0; i < n_remaining; i++) { - S_r(i, remaining_inds[i]) = 1; - } - { - int i = 0; - for (auto ind_j : active_lambda_inds) { - S_a(i, ind_j) = 1; - i++; - } - } - - for (int k = 0; k < other.N_; k++) { - Eigen::BDCSVD svd; - svd.setThreshold(1e-5); - svd.compute(S_a * other.F_[k] * S_a.transpose(), - Eigen::ComputeFullU | Eigen::ComputeFullV); - - // F_active likely to be low-rank due to friction, but that should be OK - // MatrixXd res = svd.solve(F_ar); - - // Build new complementarity constraints - // F_a_inv = pinv(S_a * F * S_a^T) - // 0 <= \lambda_k \perp E_k x_k + F_k \lambda_k + H_k u_k + c_k - // 0 = S_a *(E x + F S_a^T \lambda_a + F S_r^T \lambda_r + H_k u_k + c_k) - // \lambda_a = -F_a_inv * (S_a F S_r^T * lambda_r + S_a E x + S_a H u + S_a - // c) - // - // 0 <= \lambda_r \perp S_r (I - F S_a^T F_a_inv S_a) E x + ... - // S_r (I - F S_a^T F_a_inv S_a) F S_r^T \lambda_r + - // ... S_r (I - F S_a^T F_a_inv S_a) H u + ... S_r (I - - // F S_a^T F_a_inv S_a) c - // - // Calling L = S_r (I - F S_a^T F_a_inv S_a)S_r * other.D_[k] - // E_k = L E - // F_k = L F S_r^t - // H_k = L H - // c_k = L c - // std::cout << S_r << std::endl << std::endl; - // std::cout << other.F_[k] << std::endl << std::endl; - // std::cout << other.F_[k] << std::endl << std::endl; - // auto tmp = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - - // other.F_[k] * S_a.transpose()); - MatrixXd L = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - - other.F_[k] * S_a.transpose() * svd.solve(S_a)); - MatrixXd E_k = L * other.E_[k]; - MatrixXd F_k = L * other.F_[k] * S_r.transpose(); - MatrixXd H_k = L * other.H_[k]; - MatrixXd c_k = L * other.c_[k]; - - // Similarly, - // A_k = A - D * S_a^T * F_a_inv * S_a * E - // B_k = B - D * S_a^T * F_a_inv * S_a * H - // D_k = D * S_r^T - D * S_a^T * F_a_inv * S_a F S_r^T - // d_k = d - D * S_a^T F_a_inv * S_a * c - // - // Calling P = D * S_a^T * F_a_inv * S_a - // - // A_k = A - P E - // B_k = B - P H - // D_k = S_r D - P S_r^T - // d_k = d - P c - MatrixXd P = other.D_[k] * S_a.transpose() * svd.solve(S_a); - MatrixXd A_k = other.A_[k] - P * other.E_[k]; - MatrixXd B_k = other.B_[k] - P * other.H_[k]; - MatrixXd D_k = other.D_[k] * S_r.transpose() - P * S_r.transpose(); - MatrixXd d_k = other.d_[k] - P * other.c_[k]; - E.push_back(E_k); - F.push_back(F_k); - H.push_back(H_k); - c.push_back(c_k); - A.push_back(A_k); - B.push_back(B_k); - D.push_back(D_k); - d.push_back(d_k); - } - return LCS(A, B, D, d, E, F, H, c, other.dt_); -} - -vector> LCSFactory::PreProcessor( - const MultibodyPlant& plant, const Context& context, - const vector>>& contact_geoms, - const vector& resolve_contacts_to_list, - int num_friction_directions, bool verbose) { - - int n_contacts = std::accumulate( - resolve_contacts_to_list.begin(), resolve_contacts_to_list.end(), 0); - std::vector> resolved_contacts; - resolved_contacts.reserve(n_contacts); - - for (int i = 0; i < contact_geoms.size(); i++) { - DRAKE_DEMAND(contact_geoms[i].size() >= resolve_contacts_to_list[i]); - - const auto& candidates = contact_geoms[i]; - const int num_to_select = resolve_contacts_to_list[i]; - - if (verbose && candidates.size() > 1) { - std::cout << "Contact pair " << i << " : choosing " << num_to_select << - " among:" << std::endl; - } - - std::vector distances; - distances.reserve(candidates.size()); - for (const auto& pair : candidates) { - multibody::GeomGeomCollider collider(plant, pair); - auto [phi_i, J_i] = collider.EvalPolytope(context, - num_friction_directions); - distances.push_back(phi_i); - - if (verbose) { - PrintVerboseContactInfo(plant, context, pair, phi_i); - } - } - - for (int j = 0; j < num_to_select; ++j) { - auto min_it = std::min_element(distances.begin(), distances.end()); - int min_index = std::distance(distances.begin(), min_it); - resolved_contacts.push_back(candidates[min_index]); - distances[min_index] = std::numeric_limits::infinity(); - - if (verbose && candidates.size() > 1) { - std::cout << " --> Chose option " << min_index << std::endl; - } - } - } - - DRAKE_DEMAND(resolved_contacts.size() == n_contacts); - return resolved_contacts; -} - -void LCSFactory::PrintVerboseContactInfo(const MultibodyPlant& plant, - const Context& context, - const SortedPair& pair, - const double phi_i) { - const auto& query_port = plant.get_geometry_query_input_port(); - const auto& query_object = - query_port.template Eval>(context); - const auto& inspector = query_object.inspector(); - - // Get the witness points on each geometry. - const SignedDistancePair signed_distance_pair = - query_object.ComputeSignedDistancePairClosestPoints( - pair.first(), pair.second()); - - const Eigen::Vector3d& p_ACa = - inspector.GetPoseInFrame(pair.first()).template cast() * - signed_distance_pair.p_ACa; - const Eigen::Vector3d& p_BCb = - inspector.GetPoseInFrame(pair.second()).template cast() * - signed_distance_pair.p_BCb; - - // Represent the witness points as points in world frame. - RigidTransform T_body1_contact = RigidTransform(p_ACa); - const FrameId f1_id = inspector.GetFrameId(pair.first()); - const Body* body1 = plant.GetBodyFromFrameId(f1_id); - RigidTransform T_world_body1 = body1->EvalPoseInWorld(context); - Eigen::Vector3d p_world_contact_a = T_world_body1 * - T_body1_contact.translation(); - - RigidTransform T_body2_contact = RigidTransform(p_BCb); - const FrameId f2_id = inspector.GetFrameId(pair.second()); - const Body* body2 = plant.GetBodyFromFrameId(f2_id); - RigidTransform T_world_body2 = body2->EvalPoseInWorld(context); - Eigen::Vector3d p_world_contact_b = T_world_body2 * - T_body2_contact.translation(); - - std::cout << "Contact pair: (" << inspector.GetName(pair.first()) - << ", " << inspector.GetName(pair.second()) - << ") with phi = " << phi_i << " between world points [" - << p_world_contact_a.transpose() << "], [" - << p_world_contact_b.transpose() << "]" << std::endl; -} - -} // namespace solvers -} // namespace dairlib diff --git a/solvers/lcs_factory.h b/solvers/lcs_factory.h deleted file mode 100644 index 60ac2c1577..0000000000 --- a/solvers/lcs_factory.h +++ /dev/null @@ -1,115 +0,0 @@ -#pragma once - -#include - -#include "solvers/lcs.h" -#include "multibody/geom_geom_collider.h" -#include "drake/geometry/query_object.h" -#include "drake/geometry/geometry_ids.h" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/math/rigid_transform.h" - -using std::vector; -using drake::SortedPair; -using drake::geometry::GeometryId; -using drake::multibody::MultibodyPlant; -using drake::multibody::Body; -using drake::math::RigidTransform; -using drake::geometry::FrameId; -using drake::systems::Context; -using drake::geometry::SignedDistancePair; -namespace dairlib { -namespace solvers { - -enum class ContactModel { - kStewartAndTrinkle, /// Stewart and Trinkle timestepping contact - kAnitescu /// Anitescu convex contact -}; - -class LCSFactory { - public: - /// Build a time-invariant LCS, linearizing a MultibodyPlant - /// Contacts are specified by the pairs in contact_geoms. Each element - /// in the contact_geoms vector defines a collision. - /// This method also uses two copies of the Context, one for double and one - /// for AutoDiff. Given that Contexts can be expensive to create, this is - /// preferred to extracting the double-version from the AutoDiff. - /// - /// TODO: add variant allowing for different frictional properties per - /// contact - /// - /// @param plant The standard MultibodyPlant - /// @param context The context about which to linearize (double) - /// @param plant_ad An AutoDiffXd templated plant for gradient calculation - /// @param context The context about which to linearize (AutoDiffXd) - /// @param contact_geoms - /// @param num_friction faces - /// @param mu - /// @oaram dt The timestep for discretization - /// @param N - static LCS LinearizePlantToLCS( - const drake::multibody::MultibodyPlant& plant, - const drake::systems::Context& context, - const drake::multibody::MultibodyPlant& plant_ad, - const drake::systems::Context& context_ad, - const std::vector>& - contact_geoms, - const std::vector& mu, double dt, - int N, int n_lambda_with_tangential, - const std::vector& num_friction_directions_per_contact, - const std::vector& starting_index_per_contact_in_lambda_t_vector, - ContactModel = ContactModel::kStewartAndTrinkle); - - static std::pair> ComputeContactJacobian( - const drake::multibody::MultibodyPlant& plant, - const drake::systems::Context& context, - const std::vector>& - contact_geoms, - const std::vector& mu, int n_lambda_with_tangential, - const std::vector& num_friction_directions_per_contact, - const std::vector& starting_index_per_contact_in_lambda_t_vector, - ContactModel = ContactModel::kStewartAndTrinkle); - - /// Create an LCS by fixing some modes from another LCS - /// Ignores generated inequalities that correspond to these modes, but - /// these could be returned via pointer if useful - /// - /// @param active_lambda_inds The indices for lambda thta must be non-zero - /// @param inactive_lambda_inds The indices for lambda that must be 0 - static LCS FixSomeModes(const LCS& other, std::set active_lambda_inds, - std::set inactive_lambda_inds); - - /// Optionally preprocess contact pairs to select the closest contacts - /// @param plant The MultibodyPlant - /// @param context The plant context - /// @param contact_geoms The contact geometries - /// @param resolve_contacts_to_list The number of contacts to resolve to - /// @param num_friction_directions The number of friction directions - /// @param verbose Whether to print verbose information - /// @return The closest contacts - static std::vector> - PreProcessor( - const drake::multibody::MultibodyPlant& plant, - const drake::systems::Context& context, - const std::vector>>& - contact_geoms, - const std::vector& resolve_contacts_to_list, - int num_friction_directions, - bool verbose = false); - - private: - /// This function is for debugging purposes. This is largely a copy of the - /// EvalPolytope function in the GeomGeomCollider class with more verbosity. - /// @param plant The MultibodyPlant - /// @param context The plant context - /// @param pair The contact pair - /// @param phi_i The signed distance - static void PrintVerboseContactInfo( - const drake::multibody::MultibodyPlant& plant, - const drake::systems::Context& context, - const drake::SortedPair& pair, - const double phi_i); -}; - -} // namespace solvers -} // namespace dairlib diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index b76ad0be56..0824afd966 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -13,14 +13,6 @@ cc_library( ], ) -cc_library( - name = "c3_systems", - deps = [ - ":c3_controller", - ":lcs_factory_system", - ], -) - cc_library( name = "control_utils", srcs = [ @@ -59,34 +51,6 @@ cc_library( ], ) -cc_library( - name = "lcs_factory_system", - srcs = ["c3/lcs_factory_system.cc"], - hdrs = ["c3/lcs_factory_system.h"], - deps = [ - "//multibody:utils", - "//solvers:c3", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "c3_controller", - srcs = ["c3/c3_controller.cc"], - hdrs = ["c3/c3_controller.h"], - deps = [ - "//lcm:lcm_trajectory_saver", - "//lcmtypes:lcmt_robot", - "//multibody:utils", - "//solvers:c3", - "//solvers:c3_output", - "//solvers:solver_options_io", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) - cc_library( name = "sampling_c3_controller", srcs = ["sampling_based_c3_controller.cc"], @@ -95,15 +59,12 @@ cc_library( "//lcm:lcm_trajectory_saver", "//lcmtypes:lcmt_robot", "//multibody:utils", - "//solvers:c3", - "//solvers:c3_output", "//solvers:solver_options_io", ":face", "//systems/framework:vector", "//examples/sampling_c3:generate_samples", "//examples/sampling_c3:reposition", "//common:quaternion_error_hessian", - "//solvers:lcs", "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", "//examples/sampling_c3/parameter_headers:sampling_c3_options", "//examples/sampling_c3/parameter_headers:sampling_params", diff --git a/systems/controllers/c3/c3_controller.cc b/systems/controllers/c3/c3_controller.cc deleted file mode 100644 index 83617d02c9..0000000000 --- a/systems/controllers/c3/c3_controller.cc +++ /dev/null @@ -1,279 +0,0 @@ -#include "c3_controller.h" -#include - -#include "solvers/c3_miqp.h" -#include "solvers/c3_qp.h" -#include "solvers/lcs_factory.h" - -namespace dairlib { - -using drake::multibody::ModelInstanceIndex; -using drake::systems::BasicVector; -using drake::systems::Context; -using drake::systems::DiscreteValues; -using Eigen::MatrixXd; -using Eigen::MatrixXf; -using Eigen::VectorXd; -using Eigen::VectorXf; -using solvers::C3Base; -using solvers::C3MIQP; -using solvers::C3QP; -using solvers::LCS; -using solvers::LCSFactory; -using std::vector; -using systems::TimestampedVector; - -namespace systems { - -C3Controller::C3Controller( - const drake::multibody::MultibodyPlant& plant, C3Options c3_options) - : plant_(plant), - c3_options_(std::move(c3_options)), - G_(std::vector(c3_options_.N, c3_options_.G)), - U_(std::vector(c3_options_.N, c3_options_.U)), - N_(c3_options_.N) { - this->set_name("c3_controller"); - - double discount_factor = 1; - for (int i = 0; i < N_; ++i) { - Q_.push_back(discount_factor * c3_options_.Q); - R_.push_back(discount_factor * c3_options_.R); - discount_factor *= c3_options_.gamma; - } - Q_.push_back(discount_factor * c3_options_.Q); - DRAKE_DEMAND(Q_.size() == N_ + 1); - DRAKE_DEMAND(R_.size() == N_); - - n_q_ = plant_.num_positions(); - n_v_ = plant_.num_velocities(); - n_u_ = plant_.num_actuators(); - n_x_ = n_q_ + n_v_; - dt_ = c3_options_.dt; - solve_time_filter_constant_ = c3_options_.solve_time_filter_alpha; - if (c3_options_.contact_model == "stewart_and_trinkle") { - n_lambda_ = - 2 * c3_options_.num_contacts + - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } else if (c3_options_.contact_model == "anitescu") { - n_lambda_ = - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } - VectorXd zeros = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); - - n_u_ = plant_.num_actuators(); - - // Creates placeholder lcs to construct base C3 problem - // Placeholder LCS will have correct size as it's already determined by the - // contact model - auto lcs_placeholder = CreatePlaceholderLCS(); - auto x_desired_placeholder = - std::vector(N_ + 1, VectorXd::Zero(n_x_)); - if (c3_options_.projection_type == "MIQP") { - c3_ = std::make_unique(lcs_placeholder, - C3Base::CostMatrices(Q_, R_, G_, U_), - x_desired_placeholder, c3_options_); - - } else if (c3_options_.projection_type == "QP") { - c3_ = std::make_unique(lcs_placeholder, - C3Base::CostMatrices(Q_, R_, G_, U_), - x_desired_placeholder, c3_options_); - - } else { - std::cerr << ("Unknown projection type") << std::endl; - DRAKE_THROW_UNLESS(false); - } - - c3_->SetOsqpSolverOptions(solver_options_); - - // Set actor bounds, - // TODO(yangwill): move this out of here because it is task specific - for (int i = 0; i < c3_options_.workspace_limits.size(); ++i) { - Eigen::RowVectorXd A = VectorXd::Zero(n_x_); - A.segment(0, 3) = c3_options_.workspace_limits[i].segment(0, 3); - c3_->AddLinearConstraint(A, c3_options_.workspace_limits[i][3], - c3_options_.workspace_limits[i][4], 1); - } - for (int i : vector({0, 1})) { - Eigen::RowVectorXd A = VectorXd::Zero(n_u_); - A(i) = 1.0; - c3_->AddLinearConstraint(A, c3_options_.u_horizontal_limits[0], - c3_options_.u_horizontal_limits[1], 2); - } - for (int i : vector({2})) { - Eigen::RowVectorXd A = VectorXd::Zero(n_u_); - A(i) = 1.0; - c3_->AddLinearConstraint(A, c3_options_.u_vertical_limits[0], - c3_options_.u_vertical_limits[1], 2); - } - - lcs_state_input_port_ = - this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) - .get_index(); - lcs_input_port_ = - this->DeclareAbstractInputPort("lcs", drake::Value(lcs_placeholder)) - .get_index(); - - target_input_port_ = - this->DeclareVectorInputPort("x_lcs_des", n_x_).get_index(); - - auto c3_solution = C3Output::C3Solution(); - c3_solution.x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); - c3_solution.lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); - c3_solution.u_sol_ = MatrixXf::Zero(n_u_, N_); - c3_solution.time_vector_ = VectorXf::Zero(N_); - auto c3_intermediates = C3Output::C3Intermediates(); - c3_intermediates.z_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); - c3_intermediates.delta_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); - c3_intermediates.w_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); - c3_intermediates.time_vector_ = VectorXf::Zero(N_); - c3_solution_port_ = - this->DeclareAbstractOutputPort("c3_solution", c3_solution, - &C3Controller::OutputC3Solution) - .get_index(); - c3_intermediates_port_ = - this->DeclareAbstractOutputPort("c3_intermediates", c3_intermediates, - &C3Controller::OutputC3Intermediates) - .get_index(); - - plan_start_time_index_ = DeclareDiscreteState(1); - x_pred_index_ = DeclareDiscreteState(n_x_); - filtered_solve_time_index_ = DeclareDiscreteState(1); - - if (c3_options_.publish_frequency > 0) { - DeclarePeriodicDiscreteUpdateEvent(1 / c3_options_.publish_frequency, 0.0, - &C3Controller::ComputePlan); - } else { - DeclareForcedDiscreteUpdateEvent(&C3Controller::ComputePlan); - } -} - -LCS C3Controller::CreatePlaceholderLCS() const { - MatrixXd A = MatrixXd::Ones(n_x_, n_x_); - MatrixXd B = MatrixXd::Zero(n_x_, n_u_); - VectorXd d = VectorXd::Zero(n_x_); - MatrixXd D = MatrixXd::Ones(n_x_, n_lambda_); - MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); - MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); - MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); - VectorXd c = VectorXd::Zero(n_lambda_); - return LCS(A, B, D, d, E, F, H, c, c3_options_.N, c3_options_.dt); -} - -drake::systems::EventStatus C3Controller::ComputePlan( - const Context& context, - DiscreteValues* discrete_state) const { - auto start = std::chrono::high_resolution_clock::now(); - const BasicVector& x_des = - *this->template EvalVectorInput(context, target_input_port_); - const TimestampedVector* lcs_x = - (TimestampedVector*)this->EvalVectorInput(context, - lcs_state_input_port_); - - auto& lcs = - this->EvalAbstractInput(context, lcs_input_port_)->get_value(); - drake::VectorX x_lcs = lcs_x->get_data(); - auto& x_pred = context.get_discrete_state(x_pred_index_).value(); - auto mutable_x_pred = discrete_state->get_mutable_value(x_pred_index_); - auto mutable_solve_time = - discrete_state->get_mutable_value(filtered_solve_time_index_); - - if (x_lcs.segment(n_q_, 3).norm() > 0.01 && c3_options_.use_predicted_x0 && - !x_pred.isZero()) { - x_lcs[0] = std::clamp(x_pred[0], x_lcs[0] - 10 * dt_ * dt_, - x_lcs[0] + 10 * dt_ * dt_); - x_lcs[1] = std::clamp(x_pred[1], x_lcs[1] - 10 * dt_ * dt_, - x_lcs[1] + 10 * dt_ * dt_); - x_lcs[2] = std::clamp(x_pred[2], x_lcs[2] - 10 * dt_ * dt_, - x_lcs[2] + 10 * dt_ * dt_); - x_lcs[n_q_ + 0] = std::clamp(x_pred[n_q_ + 0], x_lcs[n_q_ + 0] - 10 * dt_, - x_lcs[n_q_ + 0] + 10 * dt_); - x_lcs[n_q_ + 1] = std::clamp(x_pred[n_q_ + 1], x_lcs[n_q_ + 1] - 10 * dt_, - x_lcs[n_q_ + 1] + 10 * dt_); - x_lcs[n_q_ + 2] = std::clamp(x_pred[n_q_ + 2], x_lcs[n_q_ + 2] - 10 * dt_, - x_lcs[n_q_ + 2] + 10 * dt_); - } - - discrete_state->get_mutable_value(plan_start_time_index_)[0] = - lcs_x->get_timestamp(); - - std::vector x_desired = - std::vector(N_ + 1, x_des.value()); - - // Force Checking of Workspace Limits - for (int i = 0; i < c3_options_.workspace_limits.size(); ++i) { - DRAKE_DEMAND(lcs_x->get_data().segment(0, 3).transpose() * - c3_options_.workspace_limits[i].segment(0, 3) > - c3_options_.workspace_limits[i][3] - - c3_options_.workspace_margins); - DRAKE_DEMAND(lcs_x->get_data().segment(0, 3).transpose() * - c3_options_.workspace_limits[i].segment(0, 3) < - c3_options_.workspace_limits[i][4] + - c3_options_.workspace_margins); - } - - c3_->UpdateLCS(lcs); - c3_->UpdateTarget(x_desired); - c3_->Solve(x_lcs); - - auto finish = std::chrono::high_resolution_clock::now(); - auto elapsed = finish - start; - double solve_time = - std::chrono::duration_cast(elapsed).count() / - 1e6; - mutable_solve_time[0] = (1 - solve_time_filter_constant_) * solve_time + - solve_time_filter_constant_ * mutable_solve_time[0]; - - if (c3_options_.publish_frequency > 0) { - solve_time = 1.0 / c3_options_.publish_frequency; - mutable_solve_time[0] = solve_time; - } - - auto z_sol = c3_->GetFullSolution(); - if (mutable_solve_time[0] < (N_ - 1) * dt_) { - int index = mutable_solve_time[0] / dt_; - double weight = (mutable_solve_time[0] - index * dt_) / dt_; - mutable_x_pred = (1 - weight) * z_sol[index].segment(0, n_x_) + - weight * z_sol[index + 1].segment(0, n_x_); - } else { - mutable_x_pred = z_sol[N_ - 1].segment(0, n_x_); - } - - return drake::systems::EventStatus::Succeeded(); -} - -void C3Controller::OutputC3Solution( - const drake::systems::Context& context, - C3Output::C3Solution* c3_solution) const { - double t = context.get_discrete_state(plan_start_time_index_)[0]; - double solve_time = context.get_discrete_state(filtered_solve_time_index_)[0]; - - auto z_sol = c3_->GetFullSolution(); - for (int i = 0; i < N_; i++) { - c3_solution->time_vector_(i) = solve_time + t + i * dt_; - c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); - c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); - c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); - } -} - -void C3Controller::OutputC3Intermediates( - const drake::systems::Context& context, - C3Output::C3Intermediates* c3_intermediates) const { - double solve_time = context.get_discrete_state(filtered_solve_time_index_)[0]; - double t = context.get_discrete_state(plan_start_time_index_)[0] + solve_time; - auto z = c3_->GetFullSolution(); - auto delta = c3_->GetDualDeltaSolution(); - auto w = c3_->GetDualWSolution(); - - for (int i = 0; i < N_; i++) { - c3_intermediates->time_vector_(i) = solve_time + t + i * dt_; - c3_intermediates->z_.col(i) = z[i].cast(); - c3_intermediates->delta_.col(i) = delta[i].cast(); - c3_intermediates->w_.col(i) = w[i].cast(); - } -} - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/c3/c3_controller.h b/systems/controllers/c3/c3_controller.h deleted file mode 100644 index c151bd8beb..0000000000 --- a/systems/controllers/c3/c3_controller.h +++ /dev/null @@ -1,104 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "common/find_resource.h" -#include "dairlib/lcmt_saved_traj.hpp" -#include "dairlib/lcmt_timestamped_saved_traj.hpp" -#include "lcm/lcm_trajectory.h" -#include "solvers/base_c3.h" -#include "solvers/c3_options.h" -#include "solvers/c3_output.h" -#include "solvers/lcs.h" -#include "solvers/solver_options_io.h" -#include "systems/framework/timestamped_vector.h" - -#include "drake/systems/framework/leaf_system.h" - -namespace dairlib { -namespace systems { - -class C3Controller : public drake::systems::LeafSystem { - public: - explicit C3Controller(const drake::multibody::MultibodyPlant& plant, - C3Options c3_options); - - const drake::systems::InputPort& get_input_port_target() const { - return this->get_input_port(target_input_port_); - } - - const drake::systems::InputPort& get_input_port_lcs_state() const { - return this->get_input_port(lcs_state_input_port_); - } - - const drake::systems::InputPort& get_input_port_lcs() const { - return this->get_input_port(lcs_input_port_); - } - - const drake::systems::OutputPort& get_output_port_c3_solution() - const { - return this->get_output_port(c3_solution_port_); - } - const drake::systems::OutputPort& get_output_port_c3_intermediates() - const { - return this->get_output_port(c3_intermediates_port_); - } - - void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options) { - solver_options_ = options; - c3_->SetOsqpSolverOptions(solver_options_); - } - - private: - solvers::LCS CreatePlaceholderLCS() const; - - drake::systems::EventStatus ComputePlan( - const drake::systems::Context& context, - drake::systems::DiscreteValues* discrete_state) const; - - void OutputC3Solution(const drake::systems::Context& context, - C3Output::C3Solution* c3_solution) const; - - void OutputC3Intermediates(const drake::systems::Context& context, - C3Output::C3Intermediates* c3_intermediates) const; - - drake::systems::InputPortIndex target_input_port_; - drake::systems::InputPortIndex lcs_state_input_port_; - drake::systems::InputPortIndex lcs_input_port_; - drake::systems::OutputPortIndex c3_solution_port_; - drake::systems::OutputPortIndex c3_intermediates_port_; - - const drake::multibody::MultibodyPlant& plant_; - - C3Options c3_options_; - drake::solvers::SolverOptions solver_options_ = - drake::yaml::LoadYamlFile( - "solvers/osqp_options_default.yaml") - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); - - // convenience for variable sizes - int n_q_; - int n_v_; - int n_x_; - int n_lambda_; - int n_u_; - double dt_; - - mutable std::unique_ptr c3_; - - double solve_time_filter_constant_; - drake::systems::DiscreteStateIndex plan_start_time_index_; - drake::systems::DiscreteStateIndex x_pred_index_; - drake::systems::DiscreteStateIndex filtered_solve_time_index_; - std::vector Q_; - std::vector R_; - std::vector G_; - std::vector U_; - int N_; -}; - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/c3/lcs_factory_system.cc b/systems/controllers/c3/lcs_factory_system.cc deleted file mode 100644 index 6e1910ff50..0000000000 --- a/systems/controllers/c3/lcs_factory_system.cc +++ /dev/null @@ -1,148 +0,0 @@ -#include "lcs_factory_system.h" - -#include - -#include "multibody/multibody_utils.h" -#include "solvers/lcs.h" -#include "solvers/lcs_factory.h" -#include "systems/framework/timestamped_vector.h" - -namespace dairlib { - -using drake::multibody::ModelInstanceIndex; -using drake::systems::BasicVector; -using drake::systems::Context; -using drake::systems::DiscreteValues; -using Eigen::MatrixXd; -using Eigen::MatrixXf; -using Eigen::VectorXd; -using solvers::LCS; -using solvers::LCSFactory; -using systems::TimestampedVector; - -namespace systems { - -LCSFactorySystem::LCSFactorySystem( - const drake::multibody::MultibodyPlant& plant, - drake::systems::Context& context, - const drake::multibody::MultibodyPlant& plant_ad, - drake::systems::Context& context_ad, - const std::vector> - contact_geoms, - C3Options c3_options) - : plant_(plant), - context_(context), - plant_ad_(plant_ad), - context_ad_(context_ad), - contact_pairs_(contact_geoms), - c3_options_(std::move(c3_options)), - N_(c3_options_.N) { - this->set_name("lcs_factory_system"); - - n_q_ = plant_.num_positions(); - n_v_ = plant_.num_velocities(); - n_x_ = n_q_ + n_v_; - dt_ = c3_options_.dt; - if (c3_options_.contact_model == "stewart_and_trinkle") { - n_lambda_ = - 2 * c3_options_.num_contacts + - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } else if (c3_options_.contact_model == "anitescu") { - n_lambda_ = - 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; - } - n_u_ = plant_.num_actuators(); - - lcs_state_input_port_ = - this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) - .get_index(); - -// lcs_inputs_input_port_ = -// this->DeclareVectorInputPort("u_lcs", BasicVector(n_u_)) -// .get_index(); - - MatrixXd A = MatrixXd::Zero(n_x_, n_x_); - MatrixXd B = MatrixXd::Zero(n_x_, n_u_); - VectorXd d = VectorXd::Zero(n_x_); - MatrixXd D = MatrixXd::Zero(n_x_, n_lambda_); - MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); - MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); - MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); - VectorXd c = VectorXd::Zero(n_lambda_); - lcs_port_ = this->DeclareAbstractOutputPort( - "lcs", LCS(A, B, D, d, E, F, H, c, N_, dt_), - &LCSFactorySystem::OutputLCS) - .get_index(); - - lcs_contact_jacobian_port_ = this->DeclareAbstractOutputPort( - "J_lcs, p_lcs", std::pair(Eigen::MatrixXd(n_x_, n_lambda_), std::vector()), - &LCSFactorySystem::OutputLCSContactJacobian) - .get_index(); -} - -void LCSFactorySystem::OutputLCS(const drake::systems::Context& context, - LCS* output_lcs) const { - const TimestampedVector* lcs_x = - (TimestampedVector*)this->EvalVectorInput(context, - lcs_state_input_port_); -// const auto lcs_u = -// (BasicVector*)this->EvalVectorInput(context, -// lcs_inputs_input_port_); - DRAKE_DEMAND(lcs_x->get_data().size() == n_x_); -// DRAKE_DEMAND(lcs_u->get_value().size() == n_u_); - VectorXd q_v_u = - VectorXd::Zero(plant_.num_positions() + plant_.num_velocities() + - plant_.num_actuators()); -// q_v_u << lcs_x->get_data(), lcs_u->get_value(); - q_v_u << lcs_x->get_data(), VectorXd::Zero(n_u_); - drake::AutoDiffVecXd q_v_u_ad = drake::math::InitializeAutoDiff(q_v_u); - - plant_.SetPositionsAndVelocities(&context_, q_v_u.head(n_x_)); - multibody::SetInputsIfNew(plant_, q_v_u.tail(n_u_), &context_); - multibody::SetInputsIfNew(plant_ad_, q_v_u_ad.tail(n_u_), - &context_ad_); - solvers::ContactModel contact_model; - if (c3_options_.contact_model == "stewart_and_trinkle") { - contact_model = solvers::ContactModel::kStewartAndTrinkle; - } else if (c3_options_.contact_model == "anitescu") { - contact_model = solvers::ContactModel::kAnitescu; - } else { - throw std::runtime_error("unknown or unsupported contact model"); - } - - *output_lcs = LCSFactory::LinearizePlantToLCS( - plant_, context_, plant_ad_, context_ad_, contact_pairs_, - c3_options_.mu, c3_options_.dt, c3_options_.N, 0, {}, {}, contact_model); -} - -void LCSFactorySystem::OutputLCSContactJacobian(const drake::systems::Context& context, - std::pair>* output) const { - const TimestampedVector* lcs_x = - (TimestampedVector*)this->EvalVectorInput(context, - lcs_state_input_port_); - - VectorXd q_v_u = - VectorXd::Zero(plant_.num_positions() + plant_.num_velocities() + - plant_.num_actuators()); - // u is irrelevant in pure geometric/kinematic calculation - q_v_u << lcs_x->get_data(), VectorXd::Zero(n_u_); - - plant_.SetPositionsAndVelocities(&context_, q_v_u.head(n_x_)); - multibody::SetInputsIfNew(plant_, q_v_u.tail(n_u_), &context_); - solvers::ContactModel contact_model; - if (c3_options_.contact_model == "stewart_and_trinkle") { - contact_model = solvers::ContactModel::kStewartAndTrinkle; - } else if (c3_options_.contact_model == "anitescu") { - contact_model = solvers::ContactModel::kAnitescu; - } else { - throw std::runtime_error("unknown or unsupported contact model"); - } - - std::vector contact_points; - *output = LCSFactory::ComputeContactJacobian( - plant_, context_, contact_pairs_, - c3_options_.mu, 0, {}, {}, contact_model); -} - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/c3/lcs_factory_system.h b/systems/controllers/c3/lcs_factory_system.h deleted file mode 100644 index a267d308e0..0000000000 --- a/systems/controllers/c3/lcs_factory_system.h +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "solvers/c3_options.h" -#include "solvers/lcs.h" - -#include "drake/systems/framework/leaf_system.h" - -namespace dairlib { -namespace systems { - -/// Outputs a lcmt_timestamped_saved_traj -class LCSFactorySystem : public drake::systems::LeafSystem { - public: - explicit LCSFactorySystem( - const drake::multibody::MultibodyPlant& plant, - drake::systems::Context& context, - const drake::multibody::MultibodyPlant& plant_ad, - drake::systems::Context& context_ad, - const std::vector> contact_geoms, - C3Options c3_options); - - const drake::systems::InputPort& get_input_port_lcs_state() const { - return this->get_input_port(lcs_state_input_port_); - } - -// const drake::systems::InputPort& get_input_port_lcs_input() const { -// return this->get_input_port(lcs_inputs_input_port_); -// } - - const drake::systems::OutputPort& get_output_port_lcs() const { - return this->get_output_port(lcs_port_); - } - - const drake::systems::OutputPort& get_output_port_lcs_contact_jacobian() const { - return this->get_output_port(lcs_contact_jacobian_port_); - } - - private: - void OutputLCS(const drake::systems::Context& context, - solvers::LCS* output_traj) const; - void OutputLCSContactJacobian(const drake::systems::Context& context, - std::pair>*) const; - - drake::systems::InputPortIndex lcs_state_input_port_; -// drake::systems::InputPortIndex lcs_inputs_input_port_; - drake::systems::OutputPortIndex lcs_port_; - drake::systems::OutputPortIndex lcs_contact_jacobian_port_; - - const drake::multibody::MultibodyPlant& plant_; - drake::systems::Context& context_; - const drake::multibody::MultibodyPlant& plant_ad_; - drake::systems::Context& context_ad_; - const std::vector> - contact_pairs_; - - C3Options c3_options_; - - // convenience for variable sizes - int n_q_; - int n_v_; - int n_x_; - int n_lambda_; - int n_u_; - int N_; - double dt_; -}; - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 10f64d0696..c881e52de1 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -29,8 +29,9 @@ using c3::C3MIQP; using c3::C3Plus; using c3::C3QP; using c3::LCS; +using c3::multibody::LCSContactDescription; using c3::multibody::LCSFactory; -using dairlib::C3Output; +using c3::systems::C3Output; using drake::SortedPair; using drake::geometry::GeometryId; using drake::multibody::ModelInstanceIndex; @@ -119,8 +120,8 @@ SamplingC3Controller::SamplingC3Controller( n_lambda_ = LCSFactory::GetNumContactVariables( c3::multibody::GetContactModelMap().at( controller_params_.sampling_c3_options.contact_model), - sampling_c3_options_.num_contacts, - sampling_c3_options_.num_friction_directions_per_contact); + sampling_c3_options_.num_contacts.value(), + sampling_c3_options_.num_friction_directions_per_contact.value()); // Placeholder LCS will have correct size as it's already determined by the // contact model. @@ -253,8 +254,7 @@ SamplingC3Controller::SamplingC3Controller( c3_intermediates.w_ = MatrixXf::Zero(n_z_, N_); c3_intermediates.delta_ = MatrixXf::Zero(n_z_, N_); c3_intermediates.time_vector_ = VectorXf::Zero(N_); - auto lcs_contact_jacobian = std::pair(Eigen::MatrixXd(n_x_, n_lambda_), - std::vector()); + auto lcs_contact_descriptions = std::vector(); // Since the num_additional_samples_repos means the additional samples // to generate in addition to the prev_repositioning_target_, add 1. @@ -298,7 +298,7 @@ SamplingC3Controller::SamplingC3Controller( .get_index(); lcs_contact_jacobian_curr_plan_port_ = this->DeclareAbstractOutputPort( - "J_lcs_curr_plan, p_lcs_curr_plan", lcs_contact_jacobian, + "J_lcs_curr_plan, p_lcs_curr_plan", lcs_contact_descriptions, &SamplingC3Controller::OutputLCSContactJacobianCurrPlan) .get_index(); @@ -329,7 +329,7 @@ SamplingC3Controller::SamplingC3Controller( .get_index(); lcs_contact_jacobian_best_plan_port_ = this->DeclareAbstractOutputPort( - "J_lcs_best_plan, p_lcs_best_plan", lcs_contact_jacobian, + "J_lcs_best_plan, p_lcs_best_plan", lcs_contact_descriptions, &SamplingC3Controller::OutputLCSContactJacobianBestPlan) .get_index(); @@ -1882,7 +1882,8 @@ SamplingC3Controller::CreateLCSObjectsForSamples( GetResolvedContactPairs( plant_, *context_, contact_pairs_, sampling_c3_options_.resolve_contacts_to, - sampling_c3_options_.num_friction_directions_per_contact, verbose_); + sampling_c3_options_.num_friction_directions_per_contact.value(), + verbose_); LCS lcs_object_sample = LCSFactory(plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs, lcs_factory_options) @@ -1898,17 +1899,15 @@ SamplingC3Controller::CreateLCSObjectsForSamples( verbose_); LCSFactoryOptions lcs_factory_options_for_cost = { .contact_model = controller_params_.sampling_c3_options.contact_model, + .N = N_ * sampling_c3_options_.lcs_dt_resolution, + .dt = dt_ / sampling_c3_options_.lcs_dt_resolution, .num_contacts = resolved_contact_pairs_for_cost_simulation.size(), .spring_stiffness = 0.0, - .num_friction_directions = std::nullopt, .num_friction_directions_per_contact = sampling_c3_options_.num_friction_directions_per_contact_for_cost, - .mu = sampling_c3_options_.mu_for_cost, - .planar_normal_direction = sampling_c3_options_.planar_normal_direction, - .planar_normal_direction_per_contact = std::nullopt, - .contact_pair_configs = std::nullopt, - .N = N_ * sampling_c3_options_.lcs_dt_resolution, - .dt = dt_ / sampling_c3_options_.lcs_dt_resolution}; + .mu_per_contact = sampling_c3_options_.mu_for_cost, + .planar_normal_direction = + sampling_c3_options_.planar_normal_direction}; LCS lcs_object_sample_for_cost_simulation = LCSFactory(plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs_for_cost_simulation, @@ -2766,8 +2765,7 @@ void SamplingC3Controller::OutputC3IntermediatesCurrPlan( void SamplingC3Controller::OutputLCSContactJacobianCurrPlan( const drake::systems::Context& context, - std::pair>* - lcs_contact_jacobian) const { + std::vector* lcs_contact_descriptions) const { const TimestampedVector* lcs_x = (TimestampedVector*)this->EvalVectorInput(context, lcs_state_input_port_); @@ -2784,13 +2782,14 @@ void SamplingC3Controller::OutputLCSContactJacobianCurrPlan( resolved_contact_pairs = GetResolvedContactPairs( plant_, *context_, contact_pairs_, sampling_c3_options_.resolve_contacts_to, - sampling_c3_options_.num_friction_directions_per_contact, verbose_); + sampling_c3_options_.num_friction_directions_per_contact.value(), + verbose_); // print size of resolved_contact_pairs - *lcs_contact_jacobian = + *lcs_contact_descriptions = LCSFactory(plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs, lcs_factory_options) - .GetContactJacobianAndPoints(); + .GetContactDescriptions(); } // Output port handlers for best sample location @@ -3003,8 +3002,7 @@ void SamplingC3Controller::OutputC3IntermediatesBestPlan( void SamplingC3Controller::OutputLCSContactJacobianBestPlan( const drake::systems::Context& context, - std::pair>* - lcs_contact_jacobian) const { + std::vector* lcs_contact_descriptions) const { const TimestampedVector* lcs_x = (TimestampedVector*)this->EvalVectorInput(context, lcs_state_input_port_); @@ -3024,11 +3022,12 @@ void SamplingC3Controller::OutputLCSContactJacobianBestPlan( resolved_contact_pairs = GetResolvedContactPairs( plant_, *context_, contact_pairs_, sampling_c3_options_.resolve_contacts_to, - sampling_c3_options_.num_friction_directions_per_contact, verbose_); - *lcs_contact_jacobian = + sampling_c3_options_.num_friction_directions_per_contact.value(), + verbose_); + *lcs_contact_descriptions = LCSFactory(plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs, lcs_factory_options) - .GetContactJacobianAndPoints(); + .GetContactDescriptions(); // Revert the context. UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index 2d07bc0267..cd6ad34cae 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -14,6 +14,7 @@ #include "c3/core/solver_options_io.h" #include "c3/multibody/lcs_factory.h" #include "c3/multibody/lcs_factory_options.h" +#include "c3/systems/framework/c3_output.h" #include "common/find_resource.h" #include "common/update_context.h" #include "dairlib/lcmt_sampling_c3_debug.hpp" @@ -25,7 +26,6 @@ #include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" #include "examples/sampling_c3/parameter_headers/sampling_params.h" #include "lcm/lcm_trajectory.h" -#include "solvers/c3_output.h" #include "systems/controllers/face.h" #include "systems/framework/timestamped_vector.h" @@ -266,7 +266,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { /// Output port functions void OutputC3SolutionCurrPlan( const drake::systems::Context& context, - dairlib::C3Output::C3Solution* c3_solution) const; + c3::systems::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionCurrPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -275,14 +275,14 @@ class SamplingC3Controller : public drake::systems::LeafSystem { dairlib::lcmt_timestamped_saved_traj* output) const; void OutputC3IntermediatesCurrPlan( const drake::systems::Context& context, - dairlib::C3Output::C3Intermediates* c3_intermediates) const; + c3::systems::C3Output::C3Intermediates* c3_intermediates) const; void OutputLCSContactJacobianCurrPlan( const drake::systems::Context& context, - std::pair>* - lcs_contact_jacobian) const; + std::vector* + lcs_contact_descriptions) const; void OutputC3SolutionBestPlan( const drake::systems::Context& context, - dairlib::C3Output::C3Solution* c3_solution) const; + c3::systems::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionBestPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -291,11 +291,11 @@ class SamplingC3Controller : public drake::systems::LeafSystem { dairlib::lcmt_timestamped_saved_traj* output) const; void OutputC3IntermediatesBestPlan( const drake::systems::Context& context, - dairlib::C3Output::C3Intermediates* c3_intermediates) const; + c3::systems::C3Output::C3Intermediates* c3_intermediates) const; void OutputLCSContactJacobianBestPlan( const drake::systems::Context& context, - std::pair>* - lcs_contact_jacobian) const; + std::vector* + lcs_contact_descriptions) const; void OutputDynamicallyFeasibleCurrPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* diff --git a/systems/trajectory_optimization/BUILD.bazel b/systems/trajectory_optimization/BUILD.bazel index a8bcc809ff..ca82ba7214 100644 --- a/systems/trajectory_optimization/BUILD.bazel +++ b/systems/trajectory_optimization/BUILD.bazel @@ -62,22 +62,6 @@ cc_library( ], ) -cc_library( - name = "c3_output_systems", - srcs = [ - "c3_output_systems.cc", - ], - hdrs = [ - "c3_output_systems.h", - ], - deps = [ - "//common:eigen_utils", - "//lcmtypes:lcmt_robot", - "//solvers:c3_output", - "@drake//:drake_shared_library", - ], -) - cc_binary( name = "passive_constrained_pendulum_dircon", srcs = ["test/passive_constrained_pendulum_dircon.cc"], diff --git a/systems/trajectory_optimization/c3_output_systems.cc b/systems/trajectory_optimization/c3_output_systems.cc deleted file mode 100644 index 61691633e3..0000000000 --- a/systems/trajectory_optimization/c3_output_systems.cc +++ /dev/null @@ -1,111 +0,0 @@ -#include "c3_output_systems.h" - -#include "common/eigen_utils.h" -#include "dairlib/lcmt_force.hpp" -#include "solvers/c3_output.h" - -namespace dairlib { -namespace systems { - -using drake::systems::Context; -using drake::systems::DiscreteValues; -using Eigen::MatrixXd; -using Eigen::Quaterniond; -using Eigen::VectorXd; - -C3OutputSender::C3OutputSender() { - c3_solution_port_ = - this->DeclareAbstractInputPort("c3_solution", - drake::Value{}) - .get_index(); - c3_intermediates_port_ = - this->DeclareAbstractInputPort("c3_intermediates", - drake::Value{}) - .get_index(); - lcs_contact_info_port_ = - this->DeclareAbstractInputPort("J_lcs, p_lcs", drake::Value>>()) - .get_index(); - - this->set_name("c3_output_sender"); - lcm_c3_output_port_ = this->DeclareAbstractOutputPort( - "lcmt_c3_output", dairlib::lcmt_c3_output(), - &C3OutputSender::OutputC3Lcm) - .get_index(); - lcs_forces_output_port_ = this->DeclareAbstractOutputPort( - "lcmt_c3_force", dairlib::lcmt_c3_forces(), - &C3OutputSender::OutputC3Forces) - .get_index(); -} - -void C3OutputSender::OutputC3Lcm(const drake::systems::Context& context, - dairlib::lcmt_c3_output* output) const { - const auto& c3_solution = - this->EvalInputValue(context, c3_solution_port_); - const auto& c3_intermediates = - this->EvalInputValue(context, - c3_intermediates_port_); - - C3Output c3_output = C3Output(*c3_solution, *c3_intermediates); - *output = c3_output.GenerateLcmObject(context.get_time()); -} - -void C3OutputSender::OutputC3Forces( - const drake::systems::Context& context, - dairlib::lcmt_c3_forces* c3_forces_output) const { - const auto& c3_solution = - this->EvalInputValue(context, c3_solution_port_); - const auto& contact_info = - this->EvalInputValue>>(context, lcs_contact_info_port_); - MatrixXd J_c = contact_info->first; - int contact_force_start = c3_solution->lambda_sol_.rows() - J_c.rows(); - bool using_stewart_and_trinkle_model = contact_force_start > 0; - - auto contact_points = contact_info->second; - int forces_per_contact = contact_info->first.rows() / contact_points.size(); - - c3_forces_output->num_forces = forces_per_contact * contact_points.size(); - c3_forces_output->forces.resize(c3_forces_output->num_forces); - - int contact_var_start; - int force_index; - for (int contact_index = 0; contact_index < contact_points.size(); - ++contact_index) { - contact_var_start = contact_force_start + forces_per_contact * contact_index; - force_index = forces_per_contact * contact_index; - for (int i = 0; i < forces_per_contact; ++i) { - int contact_jacobian_row = force_index + i; // index for anitescu model - int contact_var_index = contact_var_start + i; - if (using_stewart_and_trinkle_model){ // index for stweart and trinkle model - if (i == 0){ - contact_jacobian_row = contact_index; - contact_var_index = contact_force_start + contact_index; - } else { - contact_jacobian_row = contact_points.size() + (forces_per_contact - 1) * contact_index + i - 1; - contact_var_index = contact_force_start + contact_points.size() + (forces_per_contact - 1) * contact_index + i - 1; - } - } - auto force = lcmt_force(); - force.contact_point[0] = contact_points.at(contact_index)[0]; - force.contact_point[1] = contact_points.at(contact_index)[1]; - force.contact_point[2] = contact_points.at(contact_index)[2]; - // TODO(yangwill): find a cleaner way to figure out the equivalent forces - // VISUALIZING FORCES FOR THE FIRST KNOT POINT - // 6, 7, 8 are the indices for the x,y,z components of the tray - // expressed in the world frame - force.contact_force[0] = - c3_solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(6); - force.contact_force[1] = - c3_solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(7); - force.contact_force[2] = - c3_solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(8); - c3_forces_output->forces[force_index + i] = force; - } - } - c3_forces_output->utime = context.get_time() * 1e6; -} - -} // namespace systems -} // namespace dairlib \ No newline at end of file diff --git a/systems/trajectory_optimization/c3_output_systems.h b/systems/trajectory_optimization/c3_output_systems.h deleted file mode 100644 index 1975721240..0000000000 --- a/systems/trajectory_optimization/c3_output_systems.h +++ /dev/null @@ -1,67 +0,0 @@ -#pragma once - -#include -#include - -#include "dairlib/lcmt_c3_forces.hpp" -#include "dairlib/lcmt_c3_output.hpp" - -#include "drake/systems/framework/leaf_system.h" -#include "drake/systems/lcm/lcm_interface_system.h" - -namespace dairlib { -namespace systems { - -/// Converts a OutputVector object to LCM type lcmt_robot_output -class C3OutputSender : public drake::systems::LeafSystem { - public: - C3OutputSender(); - - const drake::systems::InputPort& get_input_port_c3_solution() const { - return this->get_input_port(c3_solution_port_); - } - - const drake::systems::InputPort& get_input_port_c3_intermediates() - const { - return this->get_input_port(c3_intermediates_port_); - } - - const drake::systems::InputPort& get_input_port_lcs_contact_info() - const { - return this->get_input_port(lcs_contact_info_port_); - } - - const drake::systems::OutputPort& get_output_port_c3_debug() const { - return this->get_output_port(lcm_c3_output_port_); - } - - // LCS contact force, not actor input forces - const drake::systems::OutputPort& get_output_port_c3_force() const { - return this->get_output_port(lcs_forces_output_port_); - } - -// // LCS actor input force -// const drake::systems::OutputPort& get_output_port_next_c3_input() const { -// return this->get_output_port(lcs_inputs_output_port_); -// } - - private: - void OutputC3Lcm(const drake::systems::Context& context, - dairlib::lcmt_c3_output* output) const; - - void OutputC3Forces(const drake::systems::Context& context, - dairlib::lcmt_c3_forces* c3_forces_output) const; - -// void OutputNextC3Input(const drake::systems::Context& context, -// drake::systems::BasicVector* u_next) const; - - drake::systems::InputPortIndex c3_solution_port_; - drake::systems::InputPortIndex c3_intermediates_port_; - drake::systems::InputPortIndex lcs_contact_info_port_; - drake::systems::OutputPortIndex lcm_c3_output_port_; - drake::systems::OutputPortIndex lcs_forces_output_port_; -// drake::systems::OutputPortIndex lcs_inputs_output_port_; -}; - -} // namespace systems -} // namespace dairlib \ No newline at end of file diff --git a/systems/visualization/BUILD.bazel b/systems/visualization/BUILD.bazel index 34d0ffcff7..b7f3993072 100644 --- a/systems/visualization/BUILD.bazel +++ b/systems/visualization/BUILD.bazel @@ -16,6 +16,7 @@ cc_library( "//common:find_resource", "//lcm:lcm_trajectory_saver", "//multibody:multipose_visualizer", + "@c3//:libc3", "@drake//:drake_shared_library", ], ) diff --git a/systems/visualization/lcm_visualization_systems.cc b/systems/visualization/lcm_visualization_systems.cc index 576ae9e28e..abd9b7fc7c 100644 --- a/systems/visualization/lcm_visualization_systems.cc +++ b/systems/visualization/lcm_visualization_systems.cc @@ -1,6 +1,6 @@ #include "lcm_visualization_systems.h" -#include +#include #include #include @@ -55,8 +55,9 @@ drake::systems::EventStatus LcmTrajectoryDrawer::DrawTrajectory( auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); - Eigen::VectorXd trajectory_block_time_vector = - PopulateTimeVectorOfLcmTrajectoryIfUnspecified(trajectory_block.time_vector); + Eigen::VectorXd trajectory_block_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + trajectory_block.time_vector); MatrixXd line_points = MatrixXd::Zero(3, N_); VectorXd breaks = VectorXd::LinSpaced(N_, trajectory_block_time_vector[0], @@ -87,9 +88,7 @@ LcmPoseDrawer::LcmPoseDrawer( const std::string& model_file, const std::string& translation_trajectory_name, const std::string& orientation_trajectory_name, - const std::string& system_name, - int num_poses, - bool add_transparency, + const std::string& system_name, int num_poses, bool add_transparency, const Eigen::VectorXd& rgb) : meshcat_(meshcat), translation_trajectory_name_(translation_trajectory_name), @@ -104,8 +103,9 @@ LcmPoseDrawer::LcmPoseDrawer( alpha_scale = 1.0 * VectorXd::Ones(N_); } - multipose_visualizers_.push_back(std::make_unique( - model_file, N_, alpha_scale, "", meshcat, system_name, rgb)); + multipose_visualizers_.push_back( + std::make_unique( + model_file, N_, alpha_scale, "", meshcat, system_name, rgb)); trajectory_input_port_ = this->DeclareAbstractInputPort( "lcmt_timestamped_saved_traj", @@ -120,15 +120,14 @@ LcmPoseDrawer::LcmPoseDrawer( std::vector model_files, std::vector translation_trajectory_names, std::vector orientation_trajectory_names, - const std::string& system_name, - int num_poses, - bool add_transparency, + const std::string& system_name, int num_poses, bool add_transparency, const Eigen::VectorXd& rgb) : meshcat_(meshcat), translation_trajectory_names_(translation_trajectory_names), orientation_trajectory_names_(orientation_trajectory_names), N_(num_poses) { - this->set_name("LcmPoseDrawer: " + system_name + translation_trajectory_names.at(0)); + this->set_name("LcmPoseDrawer: " + system_name + + translation_trajectory_names.at(0)); Eigen::VectorXd alpha_scale; if (add_transparency) { @@ -138,11 +137,11 @@ LcmPoseDrawer::LcmPoseDrawer( } for (int i = 0; i < model_files.size(); i++) { - multipose_visualizers_.push_back(std::make_unique( - model_files.at(i), N_, alpha_scale, "", meshcat, system_name, rgb)); + multipose_visualizers_.push_back( + std::make_unique( + model_files.at(i), N_, alpha_scale, "", meshcat, system_name, rgb)); } - trajectory_input_port_ = this->DeclareAbstractInputPort( "lcmt_timestamped_saved_traj", @@ -169,10 +168,11 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectory( const auto& lcm_translation_traj = lcm_traj.GetTrajectory(translation_trajectory_name_); - Eigen::VectorXd translation_time_vector = PopulateTimeVectorOfLcmTrajectoryIfUnspecified(lcm_translation_traj.time_vector); + Eigen::VectorXd translation_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + lcm_translation_traj.time_vector); auto translation_trajectory = PiecewisePolynomial::CubicHermite( - translation_time_vector, - lcm_translation_traj.datapoints.topRows(3), + translation_time_vector, lcm_translation_traj.datapoints.topRows(3), lcm_translation_traj.datapoints.bottomRows(3)); auto orientation_trajectory = PiecewiseQuaternionSlerp( {0, 1}, {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); @@ -180,7 +180,9 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectory( if (lcm_traj.HasTrajectory(orientation_trajectory_name_)) { const auto& lcm_orientation_traj = lcm_traj.GetTrajectory(orientation_trajectory_name_); - Eigen::VectorXd orientation_time_vector = PopulateTimeVectorOfLcmTrajectoryIfUnspecified(lcm_orientation_traj.time_vector); + Eigen::VectorXd orientation_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + lcm_orientation_traj.time_vector); std::vector> quaternion_datapoints; for (int i = 0; i < lcm_orientation_traj.datapoints.cols(); ++i) { VectorXd orientation_sample = lcm_orientation_traj.datapoints.col(i); @@ -201,16 +203,13 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectory( // trajectory at equal intervals based on the parameters. If the num_poses is // equal to the number of knot points, then the poses will be the same as the // knot points. - VectorXd translation_breaks = - VectorXd::LinSpaced(N_, translation_time_vector[0], - translation_time_vector.tail(1)[0]); + VectorXd translation_breaks = VectorXd::LinSpaced( + N_, translation_time_vector[0], translation_time_vector.tail(1)[0]); for (int i = 0; i < object_poses.cols(); ++i) { - object_poses.col(i) << orientation_trajectory.value( - translation_breaks(i)), + object_poses.col(i) << orientation_trajectory.value(translation_breaks(i)), translation_trajectory.value(translation_breaks(i)); } - multipose_visualizers_.at(0)->DrawPoses(object_poses); return drake::systems::EventStatus::Succeeded(); @@ -237,26 +236,25 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectoryObjects( std::vector lcm_translation_trajs; for (int i = 0; i < num_objects; i++) { LcmTrajectory::Trajectory lcm_translation_traj = - lcm_traj.GetTrajectory(translation_trajectory_names_.at(i)); - Eigen::VectorXd translation_time_vector = - PopulateTimeVectorOfLcmTrajectoryIfUnspecified(lcm_translation_traj.time_vector); - - translation_trajectory.push_back( - PiecewisePolynomial::CubicHermite( - translation_time_vector, - lcm_translation_traj.datapoints.topRows(3), - lcm_translation_traj.datapoints.bottomRows(3)) - ); + lcm_traj.GetTrajectory(translation_trajectory_names_.at(i)); + Eigen::VectorXd translation_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + lcm_translation_traj.time_vector); + + translation_trajectory.push_back(PiecewisePolynomial::CubicHermite( + translation_time_vector, lcm_translation_traj.datapoints.topRows(3), + lcm_translation_traj.datapoints.bottomRows(3))); lcm_translation_trajs.push_back(lcm_translation_traj); - } + } std::vector> orientation_trajectory; for (int i = 0; i < num_objects; i++) { orientation_trajectory.push_back(PiecewiseQuaternionSlerp( - {0, 1}, {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)})); + {0, 1}, + {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)})); } - for (int j = 0; j < num_objects; j++) { + for (int j = 0; j < num_objects; j++) { if (lcm_traj.HasTrajectory(orientation_trajectory_names_.at(j))) { const auto& lcm_orientation_traj = lcm_traj.GetTrajectory(orientation_trajectory_names_.at(j)); @@ -271,8 +269,9 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectoryObjects( orientation_sample[2], orientation_sample[3])); } } - Eigen::VectorXd orientation_time_vector = - PopulateTimeVectorOfLcmTrajectoryIfUnspecified(lcm_orientation_traj.time_vector); + Eigen::VectorXd orientation_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + lcm_orientation_traj.time_vector); orientation_trajectory.at(j) = PiecewiseQuaternionSlerp( CopyVectorXdToStdVector(orientation_time_vector), quaternion_datapoints); @@ -286,26 +285,24 @@ drake::systems::EventStatus LcmPoseDrawer::DrawTrajectoryObjects( // knot points. std::vector translation_breaks; for (int i = 0; i < num_objects; i++) { - Eigen::VectorXd translation_time_vector = - PopulateTimeVectorOfLcmTrajectoryIfUnspecified(lcm_translation_trajs.at(i).time_vector); - translation_breaks.push_back( - VectorXd::LinSpaced(N_, translation_time_vector[0], - translation_time_vector.tail(1)[0])); - } + Eigen::VectorXd translation_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + lcm_translation_trajs.at(i).time_vector); + translation_breaks.push_back(VectorXd::LinSpaced( + N_, translation_time_vector[0], translation_time_vector.tail(1)[0])); + } for (int i = 0; i < object_poses.cols(); ++i) { for (int j = 0; j < num_objects; j++) { - object_poses.col(i).segment(7 * j, 7) << - orientation_trajectory.at(j).value(translation_breaks.at(j)(i)), + object_poses.col(i).segment(7 * j, 7) + << orientation_trajectory.at(j).value(translation_breaks.at(j)(i)), translation_trajectory.at(j).value(translation_breaks.at(j)(i)); } - } for (int i = 0; i < num_objects; i++) { - multipose_visualizers_.at(i)->DrawPoses(object_poses.middleRows(7*i, 7)); - } - + multipose_visualizers_.at(i)->DrawPoses(object_poses.middleRows(7 * i, 7)); + } return drake::systems::EventStatus::Succeeded(); } @@ -330,7 +327,7 @@ LcmForceDrawer::LcmForceDrawer( force_trajectory_input_port_ = this->DeclareAbstractInputPort("lcmt_c3_forces", - drake::Value{}) + drake::Value{}) .get_index(); meshcat_->SetProperty(force_path_, "visible", true, 0); @@ -375,28 +372,30 @@ drake::systems::EventStatus LcmForceDrawer::DrawForce( lcm_traj.GetTrajectory(force_trajectory_name_); const auto& actor_trajectory_block = lcm_traj.GetTrajectory(actor_trajectory_name_); - Eigen::VectorXd force_time_vector = PopulateTimeVectorOfLcmTrajectoryIfUnspecified( - force_trajectory_block.time_vector); + Eigen::VectorXd force_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + force_trajectory_block.time_vector); auto force_trajectory = PiecewisePolynomial::FirstOrderHold( force_time_vector, force_trajectory_block.datapoints); VectorXd pose; - Eigen::VectorXd actor_time_vector = PopulateTimeVectorOfLcmTrajectoryIfUnspecified( - actor_trajectory_block.time_vector); + Eigen::VectorXd actor_time_vector = + PopulateTimeVectorOfLcmTrajectoryIfUnspecified( + actor_trajectory_block.time_vector); if (actor_trajectory_block.datapoints.rows() == 3) { auto trajectory = PiecewisePolynomial::FirstOrderHold( actor_time_vector, actor_trajectory_block.datapoints); pose = trajectory.value(robot_time); } else { auto trajectory = PiecewisePolynomial::CubicHermite( - actor_time_vector, - actor_trajectory_block.datapoints.topRows(3), + actor_time_vector, actor_trajectory_block.datapoints.topRows(3), actor_trajectory_block.datapoints.bottomRows(3)); pose = trajectory.value(robot_time); } auto force = force_trajectory.value(robot_time); const std::string& force_path_root = force_path_ + "/u_lcs/"; - meshcat_->SetTransform(force_path_root, RigidTransformd(pose), context.get_time()); + meshcat_->SetTransform(force_path_root, RigidTransformd(pose), + context.get_time()); const std::string& force_arrow_path = force_path_root + "arrow"; auto force_norm = force.norm(); @@ -404,7 +403,8 @@ drake::systems::EventStatus LcmForceDrawer::DrawForce( if (force_norm >= 0.01) { meshcat_->SetTransform( force_arrow_path, - RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), context.get_time()); + RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), + context.get_time()); const double height = force_norm / newtons_per_meter_; meshcat_->SetProperty(force_arrow_path + "/cylinder", "position", {0, 0, 0.5 * height}, context.get_time()); @@ -418,10 +418,13 @@ drake::systems::EventStatus LcmForceDrawer::DrawForce( meshcat_->SetTransform( force_arrow_path + "/head", RigidTransformd(RotationMatrixd::MakeXRotation(M_PI), - Vector3d{0, 0, height + arrowhead_height}), context.get_time()); - meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", true, context.get_time()); + Vector3d{0, 0, height + arrowhead_height}), + context.get_time()); + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", true, + context.get_time()); } else { - meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false, context.get_time()); + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false, + context.get_time()); } return drake::systems::EventStatus::Succeeded(); } @@ -429,12 +432,12 @@ drake::systems::EventStatus LcmForceDrawer::DrawForce( drake::systems::EventStatus LcmForceDrawer::DrawForces( const Context& context, DiscreteValues* discrete_state) const { - if (this->EvalInputValue( + if (this->EvalInputValue( context, force_trajectory_input_port_) ->utime < 1e-3) { return drake::systems::EventStatus::Succeeded(); } - const auto& c3_forces = this->EvalInputValue( + const auto& c3_forces = this->EvalInputValue( context, force_trajectory_input_port_); // Don't needlessly update @@ -462,12 +465,14 @@ drake::systems::EventStatus LcmForceDrawer::DrawForces( const VectorXd pose = Eigen::Map( c3_forces->forces[i].contact_point, 3); - meshcat_->SetTransform(force_path_root, RigidTransformd(pose), context.get_time()); + meshcat_->SetTransform(force_path_root, RigidTransformd(pose), + context.get_time()); // Stretch the cylinder in z. const std::string& force_arrow_path = force_path_root + "arrow"; meshcat_->SetTransform( force_arrow_path, - RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), context.get_time()); + RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), + context.get_time()); const double height = force_norm / newtons_per_meter_; meshcat_->SetProperty(force_arrow_path + "/cylinder", "position", {0, 0, 0.5 * height}, context.get_time()); @@ -481,10 +486,13 @@ drake::systems::EventStatus LcmForceDrawer::DrawForces( meshcat_->SetTransform( force_arrow_path + "/head", RigidTransformd(RotationMatrixd::MakeXRotation(M_PI), - Vector3d{0, 0, height + arrowhead_height}), context.get_time()); - meshcat_->SetProperty(force_path_root, "visible", true, context.get_time()); + Vector3d{0, 0, height + arrowhead_height}), + context.get_time()); + meshcat_->SetProperty(force_path_root, "visible", true, + context.get_time()); } else { - meshcat_->SetProperty(force_path_root, "visible", false, context.get_time()); + meshcat_->SetProperty(force_path_root, "visible", false, + context.get_time()); } } return drake::systems::EventStatus::Succeeded(); @@ -514,12 +522,12 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( // TODO(yangwill): Clean up all this visualization, move to separate // visualization directory1 - meshcat_->SetObject(c3_final_target_object_path_ + "/x-axis", cylinder_for_tray_, - {1, 0, 0, 1}); - meshcat_->SetObject(c3_final_target_object_path_ + "/y-axis", cylinder_for_tray_, - {0, 1, 0, 1}); - meshcat_->SetObject(c3_final_target_object_path_ + "/z-axis", cylinder_for_tray_, - {0, 0, 1, 1}); + meshcat_->SetObject(c3_final_target_object_path_ + "/x-axis", + cylinder_for_tray_, {1, 0, 0, 1}); + meshcat_->SetObject(c3_final_target_object_path_ + "/y-axis", + cylinder_for_tray_, {0, 1, 0, 1}); + meshcat_->SetObject(c3_final_target_object_path_ + "/z-axis", + cylinder_for_tray_, {0, 0, 1, 1}); meshcat_->SetObject(c3_target_object_path_ + "/x-axis", cylinder_for_tray_, {1, 0, 0, 0.3}); meshcat_->SetObject(c3_target_object_path_ + "/y-axis", cylinder_for_tray_, @@ -532,7 +540,7 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( {0, 1, 0, 1}); meshcat_->SetObject(c3_actual_object_path_ + "/z-axis", cylinder_for_tray_, {0, 0, 1, 1}); - if (draw_ee_){ + if (draw_ee_) { meshcat_->SetObject(c3_target_ee_path_ + "/x-axis", cylinder_for_ee_, {1, 0, 0, 1}); meshcat_->SetObject(c3_target_ee_path_ + "/y-axis", cylinder_for_ee_, @@ -564,17 +572,20 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( auto z_axis_transform_ee = RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitZ()), 0.5 * Vector3d{0.0, 0.0, 0.05}); - - meshcat_->SetTransform(c3_final_target_object_path_ + "/x-axis", x_axis_transform); - meshcat_->SetTransform(c3_final_target_object_path_ + "/y-axis", y_axis_transform); - meshcat_->SetTransform(c3_final_target_object_path_ + "/z-axis", z_axis_transform); + + meshcat_->SetTransform(c3_final_target_object_path_ + "/x-axis", + x_axis_transform); + meshcat_->SetTransform(c3_final_target_object_path_ + "/y-axis", + y_axis_transform); + meshcat_->SetTransform(c3_final_target_object_path_ + "/z-axis", + z_axis_transform); meshcat_->SetTransform(c3_target_object_path_ + "/x-axis", x_axis_transform); meshcat_->SetTransform(c3_target_object_path_ + "/y-axis", y_axis_transform); meshcat_->SetTransform(c3_target_object_path_ + "/z-axis", z_axis_transform); meshcat_->SetTransform(c3_actual_object_path_ + "/x-axis", x_axis_transform); meshcat_->SetTransform(c3_actual_object_path_ + "/y-axis", y_axis_transform); meshcat_->SetTransform(c3_actual_object_path_ + "/z-axis", z_axis_transform); - if (draw_ee_){ + if (draw_ee_) { meshcat_->SetTransform(c3_target_ee_path_ + "/x-axis", x_axis_transform_ee); meshcat_->SetTransform(c3_target_ee_path_ + "/y-axis", y_axis_transform_ee); meshcat_->SetTransform(c3_target_ee_path_ + "/z-axis", z_axis_transform_ee); @@ -587,9 +598,12 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( } LcmC3TargetDrawer::LcmC3TargetDrawer( - const std::shared_ptr& meshcat, int num_objects, bool draw_tray, - bool draw_ee) - : meshcat_(meshcat), draw_tray_(draw_tray), draw_ee_(draw_ee), num_objects_(num_objects) { + const std::shared_ptr& meshcat, int num_objects, + bool draw_tray, bool draw_ee) + : meshcat_(meshcat), + draw_tray_(draw_tray), + draw_ee_(draw_ee), + num_objects_(num_objects) { this->set_name("LcmC3TargetDrawer"); c3_state_final_target_input_port_ = this->DeclareAbstractInputPort("lcmt_c3_state: final_target", @@ -610,46 +624,47 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( for (int i = 0; i < num_objects; i++) { c3_state_paths_.push_back("c3_state_" + std::to_string(i)); - c3_final_target_object_paths_.push_back("c3_state_" + std::to_string(i) + "/c3_final_target_object"); - c3_target_object_paths_.push_back("c3_state_" + std::to_string(i) + "/c3_target_object"); - c3_actual_object_paths_.push_back("c3_state_" + std::to_string(i) + "/c3_actual_object"); - c3_target_ee_paths_.push_back("c3_state_" + std::to_string(i) + "/c3_target_ee"); - c3_actual_ee_paths_.push_back("c3_state_" + std::to_string(i) + "/c3_actual_ee"); + c3_final_target_object_paths_.push_back("c3_state_" + std::to_string(i) + + "/c3_final_target_object"); + c3_target_object_paths_.push_back("c3_state_" + std::to_string(i) + + "/c3_target_object"); + c3_actual_object_paths_.push_back("c3_state_" + std::to_string(i) + + "/c3_actual_object"); + c3_target_ee_paths_.push_back("c3_state_" + std::to_string(i) + + "/c3_target_ee"); + c3_actual_ee_paths_.push_back("c3_state_" + std::to_string(i) + + "/c3_actual_ee"); } // TODO(yangwill): Clean up all this visualization, move to separate // visualization directory1 for (int i = 0; i < num_objects_; i++) { - drake::geometry::Rgba color( - (50 * (i + 1)) % 256 / 255.0, - (100 * (i + 1)) % 256 / 255.0, - (150 * (i + 1)) % 256 / 255.0, - 1.0); - drake::geometry::Rgba color_transparent( - (50 * (i + 1)) % 256 / 255.0, - (100 * (i + 1)) % 256 / 255.0, - (150 * (i + 1)) % 256 / 255.0, - 0.3); - meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/x-axis", cylinder_for_tray_, - color); - meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/y-axis", cylinder_for_tray_, - color); - meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/z-axis", cylinder_for_tray_, - color); - meshcat_->SetObject(c3_target_object_paths_.at(i) + "/x-axis", cylinder_for_tray_, - color_transparent); - meshcat_->SetObject(c3_target_object_paths_.at(i) + "/y-axis", cylinder_for_tray_, - color_transparent); - meshcat_->SetObject(c3_target_object_paths_.at(i) + "/z-axis", cylinder_for_tray_, - color_transparent); - meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/x-axis", cylinder_for_tray_, - color); - meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/y-axis", cylinder_for_tray_, - color); - meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/z-axis", cylinder_for_tray_, - color); - } - if (draw_ee_){ + drake::geometry::Rgba color((50 * (i + 1)) % 256 / 255.0, + (100 * (i + 1)) % 256 / 255.0, + (150 * (i + 1)) % 256 / 255.0, 1.0); + drake::geometry::Rgba color_transparent((50 * (i + 1)) % 256 / 255.0, + (100 * (i + 1)) % 256 / 255.0, + (150 * (i + 1)) % 256 / 255.0, 0.3); + meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/x-axis", + cylinder_for_tray_, color); + meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/y-axis", + cylinder_for_tray_, color); + meshcat_->SetObject(c3_final_target_object_paths_.at(i) + "/z-axis", + cylinder_for_tray_, color); + meshcat_->SetObject(c3_target_object_paths_.at(i) + "/x-axis", + cylinder_for_tray_, color_transparent); + meshcat_->SetObject(c3_target_object_paths_.at(i) + "/y-axis", + cylinder_for_tray_, color_transparent); + meshcat_->SetObject(c3_target_object_paths_.at(i) + "/z-axis", + cylinder_for_tray_, color_transparent); + meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/x-axis", + cylinder_for_tray_, color); + meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/y-axis", + cylinder_for_tray_, color); + meshcat_->SetObject(c3_actual_object_paths_.at(i) + "/z-axis", + cylinder_for_tray_, color); + } + if (draw_ee_) { meshcat_->SetObject(c3_target_ee_path_ + "/x-axis", cylinder_for_ee_, {1, 0, 0, 1}); meshcat_->SetObject(c3_target_ee_path_ + "/y-axis", cylinder_for_ee_, @@ -681,19 +696,28 @@ LcmC3TargetDrawer::LcmC3TargetDrawer( auto z_axis_transform_ee = RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitZ()), 0.5 * Vector3d{0.0, 0.0, 0.05}); - + for (int i = 0; i < num_objects_; i++) { - meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/x-axis", x_axis_transform); - meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/y-axis", y_axis_transform); - meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/z-axis", z_axis_transform); - meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/x-axis", x_axis_transform); - meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/y-axis", y_axis_transform); - meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/z-axis", z_axis_transform); - meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/x-axis", x_axis_transform); - meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/y-axis", y_axis_transform); - meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/z-axis", z_axis_transform); - } - if (draw_ee_){ + meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/x-axis", + x_axis_transform); + meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/y-axis", + y_axis_transform); + meshcat_->SetTransform(c3_final_target_object_paths_.at(i) + "/z-axis", + z_axis_transform); + meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/x-axis", + x_axis_transform); + meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/y-axis", + y_axis_transform); + meshcat_->SetTransform(c3_target_object_paths_.at(i) + "/z-axis", + z_axis_transform); + meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/x-axis", + x_axis_transform); + meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/y-axis", + y_axis_transform); + meshcat_->SetTransform(c3_actual_object_paths_.at(i) + "/z-axis", + z_axis_transform); + } + if (draw_ee_) { meshcat_->SetTransform(c3_target_ee_path_ + "/x-axis", x_axis_transform_ee); meshcat_->SetTransform(c3_target_ee_path_ + "/y-axis", y_axis_transform_ee); meshcat_->SetTransform(c3_target_ee_path_ + "/z-axis", z_axis_transform_ee); @@ -711,7 +735,7 @@ drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( // Guarding the final state input port because it is not always connected, // e.g. examples/franka. const auto* c3_final_target = this->EvalInputValue( - context, c3_state_final_target_input_port_); + context, c3_state_final_target_input_port_); if (!c3_final_target || c3_final_target->utime < 1e-3) { return drake::systems::EventStatus::Succeeded(); } @@ -740,10 +764,9 @@ drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( meshcat_->SetTransform( c3_final_target_object_path_, RigidTransformd( - Eigen::Quaterniond(c3_final_target->state[3], - c3_final_target->state[4], - c3_final_target->state[5], - c3_final_target->state[6]), + Eigen::Quaterniond( + c3_final_target->state[3], c3_final_target->state[4], + c3_final_target->state[5], c3_final_target->state[6]), Vector3d{c3_final_target->state[7], c3_final_target->state[8], c3_final_target->state[9]})); meshcat_->SetTransform( @@ -752,14 +775,16 @@ drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( Eigen::Quaterniond(c3_target->state[3], c3_target->state[4], c3_target->state[5], c3_target->state[6]), Vector3d{c3_target->state[7], c3_target->state[8], - c3_target->state[9]}), context.get_time()); + c3_target->state[9]}), + context.get_time()); meshcat_->SetTransform( c3_actual_object_path_, RigidTransformd( Eigen::Quaterniond(c3_actual->state[3], c3_actual->state[4], c3_actual->state[5], c3_actual->state[6]), Vector3d{c3_actual->state[7], c3_actual->state[8], - c3_actual->state[9]}), context.get_time()); + c3_actual->state[9]}), + context.get_time()); } if (draw_ee_) { meshcat_->SetTransform( @@ -774,14 +799,13 @@ drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( return drake::systems::EventStatus::Succeeded(); } - drake::systems::EventStatus LcmC3TargetDrawer::DrawC3StateMulti( const Context& context, DiscreteValues* discrete_state) const { // Guarding the final state input port because it is not always connected, // e.g. examples/franka. const auto* c3_final_target = this->EvalInputValue( - context, c3_state_final_target_input_port_); + context, c3_state_final_target_input_port_); if (!c3_final_target || c3_final_target->utime < 1e-3) { return drake::systems::EventStatus::Succeeded(); } @@ -810,27 +834,31 @@ drake::systems::EventStatus LcmC3TargetDrawer::DrawC3StateMulti( if (draw_tray_) { meshcat_->SetTransform( c3_final_target_object_paths_.at(i), - RigidTransformd( - Eigen::Quaterniond(c3_final_target->state[3+7*i], - c3_final_target->state[4+7*i], - c3_final_target->state[5+7*i], - c3_final_target->state[6+7*i]), - Vector3d{c3_final_target->state[7+7*i], c3_final_target->state[8+7*i], - c3_final_target->state[9+7*i]})); + RigidTransformd(Eigen::Quaterniond(c3_final_target->state[3 + 7 * i], + c3_final_target->state[4 + 7 * i], + c3_final_target->state[5 + 7 * i], + c3_final_target->state[6 + 7 * i]), + Vector3d{c3_final_target->state[7 + 7 * i], + c3_final_target->state[8 + 7 * i], + c3_final_target->state[9 + 7 * i]})); meshcat_->SetTransform( c3_target_object_paths_.at(i), RigidTransformd( - Eigen::Quaterniond(c3_target->state[3+7*i], c3_target->state[4+7*i], - c3_target->state[5+7*i], c3_target->state[6+7*i]), - Vector3d{c3_target->state[7+7*i], c3_target->state[8+7*i], - c3_target->state[9+7*i]}), context.get_time()); + Eigen::Quaterniond( + c3_target->state[3 + 7 * i], c3_target->state[4 + 7 * i], + c3_target->state[5 + 7 * i], c3_target->state[6 + 7 * i]), + Vector3d{c3_target->state[7 + 7 * i], c3_target->state[8 + 7 * i], + c3_target->state[9 + 7 * i]}), + context.get_time()); meshcat_->SetTransform( c3_actual_object_paths_.at(i), RigidTransformd( - Eigen::Quaterniond(c3_actual->state[3+7*i], c3_actual->state[4+7*i], - c3_actual->state[5+7*i], c3_actual->state[6+7*i]), - Vector3d{c3_actual->state[7+7*i], c3_actual->state[8+7*i], - c3_actual->state[9+7*i]}), context.get_time()); + Eigen::Quaterniond( + c3_actual->state[3 + 7 * i], c3_actual->state[4 + 7 * i], + c3_actual->state[5 + 7 * i], c3_actual->state[6 + 7 * i]), + Vector3d{c3_actual->state[7 + 7 * i], c3_actual->state[8 + 7 * i], + c3_actual->state[9 + 7 * i]}), + context.get_time()); } } if (draw_ee_) {