diff --git a/bindings/pydairlib/systems/robot_lcm_systems_py.cc b/bindings/pydairlib/systems/robot_lcm_systems_py.cc index 8117020da2..eab647bfaf 100644 --- a/bindings/pydairlib/systems/robot_lcm_systems_py.cc +++ b/bindings/pydairlib/systems/robot_lcm_systems_py.cc @@ -8,7 +8,6 @@ #include "drake/multibody/plant/multibody_plant.h" namespace py = pybind11; -using py_rvp = py::return_value_policy; namespace dairlib { namespace pydairlib { @@ -18,31 +17,54 @@ PYBIND11_MODULE(robot_lcm_systems, m) { using drake::multibody::MultibodyPlant; using systems::RobotOutputSender; + using py_rvp = py::return_value_policy; py::class_>( m, "RobotOutputReceiver") - .def(py::init&>()); + .def(py::init&>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex>()); py::class_>( m, "RobotInputReceiver") .def(py::init&>()); py::class_>( m, "RobotOutputSender") - .def(py::init&, bool>()) + .def(py::init&, bool, bool>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex, + bool, bool>()) .def("get_input_port_state", &RobotOutputSender::get_input_port_state, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_effort", &RobotOutputSender::get_input_port_effort, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_imu", &RobotOutputSender::get_input_port_imu, - py_rvp::reference_internal); + py_rvp::reference_internal); + py::class_>( + m, "ObjectStateSender") + .def(py::init&, + drake::multibody::ModelInstanceIndex>()) + .def("get_input_port_state", &systems::ObjectStateSender::get_input_port_state, + py_rvp::reference_internal); + py::class_>( + m, "ObjectStateReceiver") + .def(py::init&>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex>()); + py::class_>( m, "RobotCommandSender") .def(py::init&>()); m.def("AddActuationRecieverAndStateSenderLcm", - &dairlib::systems::AddActuationRecieverAndStateSenderLcm, + py::overload_cast*, + const MultibodyPlant&, + drake::systems::lcm::LcmInterfaceSystem*, std::string, + std::string, double, + drake::multibody::ModelInstanceIndex, bool, double>( + &dairlib::systems::AddActuationRecieverAndStateSenderLcm), py::arg("builder"), py::arg("plant"), py::arg("lcm"), py::arg("actuator_channel"), py::arg("state_channel"), - py::arg("publish_rate"), py::arg("publish_efforts"), - py::arg("actuator_delay")); + py::arg("publish_rate"), py::arg("model_instance"), + py::arg("publish_efforts"), py::arg("actuator_delay")); } diff --git a/examples/Cassie/systems/cassie_encoder.cc b/examples/Cassie/systems/cassie_encoder.cc index eee194243e..2cec041487 100644 --- a/examples/Cassie/systems/cassie_encoder.cc +++ b/examples/Cassie/systems/cassie_encoder.cc @@ -7,6 +7,7 @@ namespace dairlib { using Eigen::VectorXd; +using drake::systems::BasicVector; CassieEncoder::CassieEncoder( const drake::multibody::MultibodyPlant& plant) @@ -37,17 +38,17 @@ CassieEncoder::CassieEncoder( } this->DeclareVectorInputPort( "robot_state", - systems::BasicVector(num_positions_ + num_velocities_)); + BasicVector(num_positions_ + num_velocities_)); this->DeclareVectorOutputPort( "filtered_state", - systems::BasicVector(num_positions_ + num_velocities_), + BasicVector(num_positions_ + num_velocities_), &CassieEncoder::UpdateFilter); } void CassieEncoder::UpdateFilter(const drake::systems::Context& context, - systems::BasicVector* output) const { - const systems::BasicVector& x = - *this->template EvalVectorInput(context, 0); + BasicVector* output) const { + const BasicVector& x = + *this->template EvalVectorInput(context, 0); VectorXd q = x.value().head(num_positions_); VectorXd v = x.value().tail(num_velocities_); diff --git a/examples/Cassie/systems/cassie_encoder.h b/examples/Cassie/systems/cassie_encoder.h index a2e5a011ca..66749179e9 100644 --- a/examples/Cassie/systems/cassie_encoder.h +++ b/examples/Cassie/systems/cassie_encoder.h @@ -70,7 +70,7 @@ class CassieEncoder final : public drake::systems::LeafSystem { protected: void UpdateFilter(const drake::systems::Context& context, - systems::BasicVector* output) const; + drake::systems::BasicVector* output) const; private: struct DriveFilter { diff --git a/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc b/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc index 84de52e132..9cb578a982 100644 --- a/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc +++ b/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc @@ -7,6 +7,7 @@ namespace systems { using std::string; using drake::systems::Context; +using drake::systems::BasicVector; using drake::AbstractValue; using dairlib::systems::TimestampedVector; using Eigen::VectorXd; diff --git a/examples/franka/BUILD.bazel b/examples/franka/BUILD.bazel new file mode 100644 index 0000000000..29d2705534 --- /dev/null +++ b/examples/franka/BUILD.bazel @@ -0,0 +1,211 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "urdfs", + data = glob([ + "urdf/**", + ]), +) + +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:controllers_all", + "//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", + "//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", + "//examples/franka/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", + "//examples/franka/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_drake_lcm_driver_channels.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([ + "*yaml", + ]), + deps = [ + "@drake//:drake_shared_library", + ], +) diff --git a/examples/franka/README.md b/examples/franka/README.md new file mode 100644 index 0000000000..989a8dc943 --- /dev/null +++ b/examples/franka/README.md @@ -0,0 +1,77 @@ +## 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/franka_bridge_driver_in.cc b/examples/franka/franka_bridge_driver_in.cc new file mode 100644 index 0000000000..c08ba518d4 --- /dev/null +++ b/examples/franka/franka_bridge_driver_in.cc @@ -0,0 +1,101 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "examples/franka/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/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, "examples/franka/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 new file mode 100644 index 0000000000..ce0e80f25f --- /dev/null +++ b/examples/franka/franka_bridge_driver_out.cc @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "examples/franka/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/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, "examples/franka/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 new file mode 100644 index 0000000000..a3ccbaafc2 --- /dev/null +++ b/examples/franka/franka_c3_controller.cc @@ -0,0 +1,334 @@ + +#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 "examples/franka/systems/c3_state_sender.h" +#include "examples/franka/systems/c3_trajectory_generator.h" +#include "examples/franka/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); + parser_tray.AddModels(scene_params.object_models[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::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_port_object_state()); + 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 new file mode 100644 index 0000000000..b20047cf2c --- /dev/null +++ b/examples/franka/franka_hardware.pmd @@ -0,0 +1,81 @@ +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 new file mode 100644 index 0000000000..7c0252e06c --- /dev/null +++ b/examples/franka/franka_lcm_ros_bridge.cc @@ -0,0 +1,125 @@ +#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 new file mode 100644 index 0000000000..742aaf4d2f --- /dev/null +++ b/examples/franka/franka_osc_controller.cc @@ -0,0 +1,287 @@ + +#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 "examples/franka/systems/end_effector_force_trajectory.h" +#include "examples/franka/systems/end_effector_orientation.h" +#include "examples/franka/systems/end_effector_trajectory.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(controller_params.neutral_position); + 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 radio_to_vector = builder.AddSystem(); + auto osc = builder.AddSystem( + plant, 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, *radio_to_vector); + builder.Connect(radio_to_vector->get_output_port(), + end_effector_trajectory->get_input_port_radio()); + builder.Connect(radio_to_vector->get_output_port(), + end_effector_orientation_trajectory->get_input_port_radio()); + builder.Connect(radio_to_vector->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 new file mode 100644 index 0000000000..f61ae024ba --- /dev/null +++ b/examples/franka/franka_ros_lcm_bridge.cc @@ -0,0 +1,191 @@ +#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 new file mode 100644 index 0000000000..f047338d8d --- /dev/null +++ b/examples/franka/franka_sim.cc @@ -0,0 +1,231 @@ +#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 new file mode 100644 index 0000000000..f12f036d8d --- /dev/null +++ b/examples/franka/franka_sim.pmd @@ -0,0 +1,69 @@ +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 new file mode 100644 index 0000000000..d370bd6d71 --- /dev/null +++ b/examples/franka/franka_visualizer.cc @@ -0,0 +1,326 @@ +#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() < 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 new file mode 100644 index 0000000000..720762fd30 --- /dev/null +++ b/examples/franka/parameters/franka_c3_controller_params.yaml @@ -0,0 +1,36 @@ +scene_index: 0 +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 new file mode 100644 index 0000000000..b136c938f3 --- /dev/null +++ b/examples/franka/parameters/franka_c3_qp_settings.yaml @@ -0,0 +1,27 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + linsys_solver: 0 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + 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_interval: 0 + 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 new file mode 100644 index 0000000000..7d4034db74 --- /dev/null +++ b/examples/franka/parameters/franka_c3_scene_params.h @@ -0,0 +1,30 @@ +#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_drake_lcm_driver_channels.h b/examples/franka/parameters/franka_drake_lcm_driver_channels.h new file mode 100644 index 0000000000..eaa7d3ee08 --- /dev/null +++ b/examples/franka/parameters/franka_drake_lcm_driver_channels.h @@ -0,0 +1,15 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + + +struct FrankaDrakeLcmDriverChannels { + std::string franka_status_channel; + std::string franka_command_channel; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_status_channel)); + a->Visit(DRAKE_NVP(franka_command_channel)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_drake_lcm_driver_channels.yaml b/examples/franka/parameters/franka_drake_lcm_driver_channels.yaml new file mode 100644 index 0000000000..55cc785c3b --- /dev/null +++ b/examples/franka/parameters/franka_drake_lcm_driver_channels.yaml @@ -0,0 +1,2 @@ +franka_status_channel: PANDA_STATUS # lcmt_panda_status +franka_command_channel: PANDA_COMMAND # lcmt_panda_command diff --git a/examples/franka/parameters/franka_lcm_channels.h b/examples/franka/parameters/franka_lcm_channels.h new file mode 100644 index 0000000000..adb84c4e2b --- /dev/null +++ b/examples/franka/parameters/franka_lcm_channels.h @@ -0,0 +1,39 @@ +#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 new file mode 100644 index 0000000000..2c1b6215f4 --- /dev/null +++ b/examples/franka/parameters/franka_osc_controller_params.h @@ -0,0 +1,100 @@ +#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 new file mode 100644 index 0000000000..8fa4ae1673 --- /dev/null +++ b/examples/franka/parameters/franka_osc_controller_params.yaml @@ -0,0 +1,68 @@ +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 new file mode 100644 index 0000000000..639eca37a0 --- /dev/null +++ b/examples/franka/parameters/franka_osc_qp_settings.yaml @@ -0,0 +1,27 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + linsys_solver: 0 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + 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_interval: 0 + 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 new file mode 100644 index 0000000000..9771db4a84 --- /dev/null +++ b/examples/franka/parameters/franka_qp_options.yaml @@ -0,0 +1,4 @@ +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 new file mode 100644 index 0000000000..0835d92e10 --- /dev/null +++ b/examples/franka/parameters/franka_sim_params.h @@ -0,0 +1,81 @@ +#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 new file mode 100644 index 0000000000..73fdfee3f0 --- /dev/null +++ b/examples/franka/parameters/franka_sim_params.yaml @@ -0,0 +1,59 @@ +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: 0 +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 new file mode 100644 index 0000000000..cc546c6b30 --- /dev/null +++ b/examples/franka/parameters/franka_sim_scene_params.h @@ -0,0 +1,25 @@ +#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 new file mode 100644 index 0000000000..a1c12f8099 --- /dev/null +++ b/examples/franka/parameters/lcm_channels_hardware.yaml @@ -0,0 +1,17 @@ +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 new file mode 100644 index 0000000000..97596064bf --- /dev/null +++ b/examples/franka/parameters/lcm_channels_simulation.yaml @@ -0,0 +1,17 @@ +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 new file mode 100644 index 0000000000..b813f26d31 --- /dev/null +++ b/examples/franka/parameters/sim_scenes/empty_scene.yaml @@ -0,0 +1,7 @@ +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 new file mode 100644 index 0000000000..6f16b5b21b --- /dev/null +++ b/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml @@ -0,0 +1,16 @@ +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 new file mode 100644 index 0000000000..1f9c99e12a --- /dev/null +++ b/examples/franka/parameters/sim_scenes/supports_scene.yaml @@ -0,0 +1,13 @@ +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 new file mode 100644 index 0000000000..336dae7191 --- /dev/null +++ b/examples/franka/parameters/sim_scenes/wall_scene.yaml @@ -0,0 +1,10 @@ +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/systems/BUILD.bazel b/examples/franka/systems/BUILD.bazel new file mode 100644 index 0000000000..77cef8a42f --- /dev/null +++ b/examples/franka/systems/BUILD.bazel @@ -0,0 +1,160 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "franka_systems", + srcs = [], + deps = [ + ":c3_state_sender", + ":c3_trajectory_generator", + ":end_effector_force_trajectory", + ":end_effector_orientation", + ":end_effector_trajectory", + ":external_force_generator", + ":franka_kinematics", + ":franka_state_translator", + ":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 = "end_effector_trajectory", + srcs = ["end_effector_trajectory.cc"], + hdrs = ["end_effector_trajectory.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "end_effector_force_trajectory", + srcs = ["end_effector_force_trajectory.cc"], + hdrs = ["end_effector_force_trajectory.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "end_effector_orientation", + srcs = ["end_effector_orientation.cc"], + hdrs = ["end_effector_orientation.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 = [ + ":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", + ], +) + +cc_library( + name = "c3_state_sender", + srcs = [ + "c3_state_sender.cc", + ], + hdrs = [ + "c3_state_sender.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_state_translator", + srcs = [ + "franka_state_translator.cc", + ], + hdrs = [ + "franka_state_translator.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_kinematics", + srcs = ["franka_kinematics.cc"], + hdrs = ["franka_kinematics.h"], + deps = [ + ":franka_kinematics_vector", + "//common", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/primitives", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_kinematics_vector", + srcs = ["franka_kinematics_vector.cc"], + hdrs = ["franka_kinematics_vector.h"], + deps = [ + "//common", + "//multibody:utils", + "//systems/primitives", + "@drake//:drake_shared_library", + ], +) diff --git a/examples/franka/systems/c3_state_sender.cc b/examples/franka/systems/c3_state_sender.cc new file mode 100644 index 0000000000..61a8b0f6bc --- /dev/null +++ b/examples/franka/systems/c3_state_sender.cc @@ -0,0 +1,61 @@ +#include "examples/franka/systems/c3_state_sender.h" +#include "systems/framework/timestamped_vector.h" + +namespace dairlib { + +using drake::systems::BasicVector; +using systems::TimestampedVector; +using drake::systems::Context; + +namespace systems { + +C3StateSender::C3StateSender(int state_size, + std::vector state_names) { + this->set_name("c3_state_sender"); + + n_x_ = state_size; + target_state_ = this->DeclareVectorInputPort("target_state", + BasicVector(state_size)) + .get_index(); + actual_state_ = this->DeclareVectorInputPort("actual_state", + TimestampedVector(state_size)) + .get_index(); + + lcmt_c3_state default_c3_state = dairlib::lcmt_c3_state(); + default_c3_state.num_states = n_x_; + default_c3_state.utime = 0; + default_c3_state.state = std::vector(n_x_); + default_c3_state.state_names = state_names; + target_c3_state_ = this->DeclareAbstractOutputPort( + "c3_actor_trajectory_output", default_c3_state, + &C3StateSender::OutputTargetState) + .get_index(); + actual_c3_state_ = this->DeclareAbstractOutputPort( + "c3_object_trajectory_output", default_c3_state, + &C3StateSender::OutputActualState) + .get_index(); +} + +void C3StateSender::OutputTargetState( + const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const { + const auto target_state = this->EvalVectorInput(context, target_state_); + DRAKE_DEMAND(target_state->size() == n_x_); + output->utime = context.get_time() * 1e6; + for (int i = 0; i < n_x_; ++i) { + output->state[i] = static_cast(target_state->GetAtIndex(i)); + } +} + +void C3StateSender::OutputActualState( + const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const { + const auto actual_state = (TimestampedVector*)this->EvalVectorInput(context, actual_state_); + DRAKE_DEMAND(actual_state->get_data().size() == n_x_); + output->utime = context.get_time() * 1e6; + for (int i = 0; i < n_x_; ++i) { + output->state[i] = static_cast(actual_state->GetAtIndex(i)); + } +} +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/c3_state_sender.h b/examples/franka/systems/c3_state_sender.h new file mode 100644 index 0000000000..5d43a1ea35 --- /dev/null +++ b/examples/franka/systems/c3_state_sender.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_c3_state.hpp" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class C3StateSender : public drake::systems::LeafSystem { + public: + explicit C3StateSender(int state_size, std::vector state_names); + + const drake::systems::InputPort& get_input_port_target_state() const { + return this->get_input_port(target_state_); + } + + const drake::systems::InputPort& get_input_port_actual_state() const { + return this->get_input_port(actual_state_); + } + + const drake::systems::OutputPort& get_output_port_target_c3_state() + const { + return this->get_output_port(target_c3_state_); + } + const drake::systems::OutputPort& get_output_port_actual_c3_state() + const { + return this->get_output_port(actual_c3_state_); + } + + private: + void OutputTargetState(const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const; + + void OutputActualState(const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const; + + drake::systems::InputPortIndex target_state_; + drake::systems::InputPortIndex actual_state_; + drake::systems::OutputPortIndex target_c3_state_; + drake::systems::OutputPortIndex actual_c3_state_; + + int n_x_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/c3_trajectory_generator.cc b/examples/franka/systems/c3_trajectory_generator.cc new file mode 100644 index 0000000000..9af7656509 --- /dev/null +++ b/examples/franka/systems/c3_trajectory_generator.cc @@ -0,0 +1,154 @@ +#include "examples/franka/systems/c3_trajectory_generator.h" + +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "examples/franka/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 new file mode 100644 index 0000000000..5ae42e5b8a --- /dev/null +++ b/examples/franka/systems/c3_trajectory_generator.h @@ -0,0 +1,72 @@ +#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/end_effector_force_trajectory.cc b/examples/franka/systems/end_effector_force_trajectory.cc new file mode 100644 index 0000000000..5daabaee6e --- /dev/null +++ b/examples/franka/systems/end_effector_force_trajectory.cc @@ -0,0 +1,86 @@ +#include "end_effector_force_trajectory.h" + +#include "dairlib/lcmt_radio_out.hpp" +#include "multibody/multibody_utils.h" + +using Eigen::Map; +using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; +using std::string; + +using dairlib::systems::OutputVector; +using drake::multibody::RigidBodyFrame; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorForceTrajectoryGenerator::EndEffectorForceTrajectoryGenerator() { + PiecewisePolynomial pp = PiecewisePolynomial(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + controller_switch_index_ = this->DeclareDiscreteState(VectorXd::Ones(1)); + DeclareForcedDiscreteUpdateEvent( + &EndEffectorForceTrajectoryGenerator::DiscreteVariableUpdate); + PiecewisePolynomial empty_pp_traj(Vector3d::Zero()); + Trajectory& traj_inst = empty_pp_traj; + this->DeclareAbstractOutputPort( + "end_effector_force_trajectory", traj_inst, + &EndEffectorForceTrajectoryGenerator::CalcTraj); +} + +EventStatus EndEffectorForceTrajectoryGenerator::DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const { + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + bool using_c3 = context.get_discrete_state(controller_switch_index_)[0]; + if (!using_c3 && radio_out->value()[14] == 0) { + if (!trajectory_input.value(0).isZero() && + (context.get_time() - trajectory_input.start_time()) < 0.04) { + discrete_state->get_mutable_value(controller_switch_index_)[0] = 1; + } + } + return EventStatus::Succeeded(); +} + +void EndEffectorForceTrajectoryGenerator::CalcTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + // // Read in finite state machine + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (radio_out->value()[11] || radio_out->value()[14] || + trajectory_input.value(0).isZero()) { + *casted_traj = + drake::trajectories::PiecewisePolynomial(Vector3d::Zero()); + } else { + if (context.get_discrete_state(controller_switch_index_)[0]) { + *casted_traj = *(PiecewisePolynomial*)dynamic_cast< + const PiecewisePolynomial*>(&trajectory_input); + } + } +} + +} // namespace dairlib diff --git a/examples/franka/systems/end_effector_force_trajectory.h b/examples/franka/systems/end_effector_force_trajectory.h new file mode 100644 index 0000000000..0c4468d228 --- /dev/null +++ b/examples/franka/systems/end_effector_force_trajectory.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "systems/framework/output_vector.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorForceTrajectoryGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorForceTrajectoryGenerator(); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcTraj(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::DiscreteStateIndex controller_switch_index_; + + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; +}; + +} // namespace dairlib diff --git a/examples/franka/systems/end_effector_orientation.cc b/examples/franka/systems/end_effector_orientation.cc new file mode 100644 index 0000000000..ac42221bcd --- /dev/null +++ b/examples/franka/systems/end_effector_orientation.cc @@ -0,0 +1,57 @@ +#include "end_effector_orientation.h" + +#include "dairlib/lcmt_radio_out.hpp" + +using drake::systems::BasicVector; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::string; + +using drake::systems::Context; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorOrientationGenerator::EndEffectorOrientationGenerator() { + auto pp = drake::trajectories::PiecewiseQuaternionSlerp(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + PiecewiseQuaternionSlerp empty_slerp_traj; + Trajectory& traj_inst = empty_slerp_traj; + this->DeclareAbstractOutputPort("end_effector_orientation", traj_inst, + &EndEffectorOrientationGenerator::CalcTraj) + .get_index(); +} + +void EndEffectorOrientationGenerator::CalcTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + auto* casted_traj = (PiecewiseQuaternionSlerp*)dynamic_cast< + PiecewiseQuaternionSlerp*>(traj); + if (radio_out->value()[14] and track_orientation_) { + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + *casted_traj = *(PiecewiseQuaternionSlerp*)dynamic_cast< + const PiecewiseQuaternionSlerp*>(&trajectory_input); + } else { + PiecewiseQuaternionSlerp result; + Eigen::VectorXd neutral_quaternion = VectorXd::Zero(4); + neutral_quaternion(0) = 1; + result = drake::trajectories::PiecewiseQuaternionSlerp( + {0, 1}, + {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); + *casted_traj = result; + } +} + +} // namespace dairlib diff --git a/examples/franka/systems/end_effector_orientation.h b/examples/franka/systems/end_effector_orientation.h new file mode 100644 index 0000000000..4799bc0752 --- /dev/null +++ b/examples/franka/systems/end_effector_orientation.h @@ -0,0 +1,38 @@ +#pragma once + +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorOrientationGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorOrientationGenerator(); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + void SetTrackOrientation(bool track_orientation) { + track_orientation_ = track_orientation; + } + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcTraj(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + bool track_orientation_; + + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; +}; + +} // namespace dairlib diff --git a/examples/franka/systems/end_effector_trajectory.cc b/examples/franka/systems/end_effector_trajectory.cc new file mode 100644 index 0000000000..a7b491667b --- /dev/null +++ b/examples/franka/systems/end_effector_trajectory.cc @@ -0,0 +1,78 @@ +#include "end_effector_trajectory.h" +#include +#include "dairlib/lcmt_radio_out.hpp" +#include "multibody/multibody_utils.h" + +using Eigen::Map; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::string; + +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorTrajectoryGenerator::EndEffectorTrajectoryGenerator(const Eigen::Vector3d& neutral_pose) { + // Input/Output Setup + PiecewisePolynomial pp = PiecewisePolynomial(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + + PiecewisePolynomial empty_pp_traj(neutral_pose); + Trajectory& traj_inst = empty_pp_traj; + this->DeclareAbstractOutputPort("end_effector_trajectory", traj_inst, + &EndEffectorTrajectoryGenerator::CalcTraj); +} + +void EndEffectorTrajectoryGenerator::SetRemoteControlParameters( + const Eigen::Vector3d& neutral_pose, double x_scale, double y_scale, + double z_scale) { + neutral_pose_ = neutral_pose; + x_scale_ = x_scale; + y_scale_ = y_scale; + z_scale_ = z_scale; +} + +void EndEffectorTrajectoryGenerator::CalcTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (radio_out->value()[14]) { + PiecewisePolynomial result; + VectorXd y_0 = neutral_pose_; + if (radio_out->value()[10]){ + y_0(0) += radio_out->value()[0] * x_scale_; + y_0(1) += radio_out->value()[1] * y_scale_; + y_0(2) += radio_out->value()[2] * z_scale_; + } + result = drake::trajectories::PiecewisePolynomial(y_0); + *casted_traj = result; + } else { + if (trajectory_input.value(0).isZero()) { + } else { + *casted_traj = *(PiecewisePolynomial*)dynamic_cast< + const PiecewisePolynomial*>(&trajectory_input); + } + } +} + +} // namespace dairlib diff --git a/examples/franka/systems/end_effector_trajectory.h b/examples/franka/systems/end_effector_trajectory.h new file mode 100644 index 0000000000..f98c443cb8 --- /dev/null +++ b/examples/franka/systems/end_effector_trajectory.h @@ -0,0 +1,37 @@ +#pragma once + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorTrajectoryGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorTrajectoryGenerator(const Eigen::Vector3d& neutral_pose); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + void SetRemoteControlParameters(const Eigen::Vector3d& neutral_pose, + double x_scale, double y_scale, + double z_scale); + + private: + void CalcTraj(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; + + Eigen::Vector3d neutral_pose_ = {0.55, 0, 0.40}; + double x_scale_; + double y_scale_; + double z_scale_; +}; + +} // namespace dairlib diff --git a/examples/franka/systems/external_force_generator.cc b/examples/franka/systems/external_force_generator.cc new file mode 100644 index 0000000000..f3168085e6 --- /dev/null +++ b/examples/franka/systems/external_force_generator.cc @@ -0,0 +1,62 @@ +#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 new file mode 100644 index 0000000000..a94f023f4b --- /dev/null +++ b/examples/franka/systems/external_force_generator.h @@ -0,0 +1,38 @@ +#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/franka_kinematics.cc b/examples/franka/systems/franka_kinematics.cc new file mode 100644 index 0000000000..4119c93c27 --- /dev/null +++ b/examples/franka/systems/franka_kinematics.cc @@ -0,0 +1,118 @@ +#include "examples/franka/systems/franka_kinematics.h" + +#include "common/find_resource.h" +#include "multibody/multibody_utils.h" + +namespace dairlib { + +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using Eigen::VectorXd; +using systems::OutputVector; +using systems::StateVector; +using systems::TimestampedVector; + +namespace systems { + +FrankaKinematics::FrankaKinematics(const MultibodyPlant& franka_plant, + Context* franka_context, + const MultibodyPlant& object_plant, + Context* object_context, + const std::string& end_effector_name, + const std::string& object_name, + bool include_end_effector_orientation) + : franka_plant_(franka_plant), + franka_context_(franka_context), + object_plant_(object_plant), + object_context_(object_context), + world_(franka_plant_.world_frame()), + end_effector_name_(end_effector_name), + object_name_(object_name), + include_end_effector_orientation_(include_end_effector_orientation) { + this->set_name("franka_kinematics"); + franka_state_port_ = + this->DeclareVectorInputPort( + "x_franka", OutputVector(franka_plant.num_positions(), + franka_plant.num_velocities(), + franka_plant.num_actuators())) + .get_index(); + + object_state_port_ = + this->DeclareVectorInputPort( + "x_object", StateVector(object_plant.num_positions(), + object_plant.num_velocities())) + .get_index(); + num_end_effector_positions_ = 3 + include_end_effector_orientation_ * 3; + num_object_positions_ = 7; + num_end_effector_velocities_ = 3 + include_end_effector_orientation_ * 3; + num_object_velocities_ = 6; + lcs_state_port_ = + this->DeclareVectorOutputPort( + "x_lcs", + FrankaKinematicsVector( + num_end_effector_positions_, num_object_positions_, + num_end_effector_velocities_, num_object_velocities_), + &FrankaKinematics::ComputeLCSState) + .get_index(); +} + +void FrankaKinematics::ComputeLCSState( + const drake::systems::Context& context, + FrankaKinematicsVector* lcs_state) const { + const OutputVector* franka_output = + (OutputVector*)this->EvalVectorInput(context, franka_state_port_); + const StateVector* object_output = + (StateVector*)this->EvalVectorInput(context, object_state_port_); + + VectorXd q_franka = franka_output->GetPositions(); + VectorXd v_franka = franka_output->GetVelocities(); + VectorXd q_object = object_output->GetPositions(); + VectorXd v_object = object_output->GetVelocities(); + multibody::SetPositionsIfNew(franka_plant_, q_franka, + franka_context_); + multibody::SetVelocitiesIfNew(franka_plant_, v_franka, + franka_context_); + multibody::SetPositionsIfNew(object_plant_, q_object, + object_context_); + multibody::SetVelocitiesIfNew(object_plant_, v_object, + object_context_); + + auto end_effector_pose = franka_plant_.EvalBodyPoseInWorld( + *franka_context_, franka_plant_.GetBodyByName(end_effector_name_)); + auto object_pose = object_plant_.EvalBodyPoseInWorld( + *object_context_, object_plant_.GetBodyByName(object_name_)); + auto end_effector_spatial_velocity = + franka_plant_.EvalBodySpatialVelocityInWorld( + *franka_context_, franka_plant_.GetBodyByName(end_effector_name_)); + auto end_effector_rotation_rpy = + end_effector_pose.rotation().ToRollPitchYaw().vector(); + VectorXd end_effector_positions = VectorXd::Zero(num_end_effector_positions_); + VectorXd end_effector_velocities = + VectorXd::Zero(num_end_effector_velocities_); + + if (num_end_effector_positions_ > 3) { + end_effector_positions << end_effector_pose.translation(), + end_effector_rotation_rpy; + } else { + end_effector_positions << end_effector_pose.translation(); + } + if (num_end_effector_velocities_ > 3) { + end_effector_velocities << end_effector_spatial_velocity.rotational(), + end_effector_spatial_velocity.translational(); + } else { + end_effector_velocities << end_effector_spatial_velocity.translational(); + } + + VectorXd object_position = q_object; + object_position << q_object.head(4), object_pose.translation(); + + lcs_state->SetEndEffectorPositions(end_effector_positions); + lcs_state->SetObjectPositions(object_position); + lcs_state->SetEndEffectorVelocities(end_effector_velocities); + lcs_state->SetObjectVelocities(v_object); + lcs_state->set_timestamp(franka_output->get_timestamp()); +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/franka_kinematics.h b/examples/franka/systems/franka_kinematics.h new file mode 100644 index 0000000000..cc57c8dee7 --- /dev/null +++ b/examples/franka/systems/franka_kinematics.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include "examples/franka/systems/franka_kinematics_vector.h" + +#include "drake/systems/framework/leaf_system.h" +#include "systems/framework/timestamped_vector.h" +#include "systems/framework/output_vector.h" +#include "systems/framework/state_vector.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class FrankaKinematics : public drake::systems::LeafSystem { + public: + explicit FrankaKinematics(const drake::multibody::MultibodyPlant& franka_plant, + drake::systems::Context* franka_context, + const drake::multibody::MultibodyPlant& object_plant, + drake::systems::Context* object_context, + const std::string& end_effector_name, + const std::string& object_name, + bool include_end_effector_orientation); + + const drake::systems::InputPort& get_input_port_object_state() const { + return this->get_input_port(object_state_port_); + } + + const drake::systems::InputPort& get_input_port_franka_state() const { + return this->get_input_port(franka_state_port_); + } + + const drake::systems::OutputPort& get_output_port_lcs_state() const { + return this->get_output_port(lcs_state_port_); + } + + private: + void ComputeLCSState( + const drake::systems::Context& context, + FrankaKinematicsVector* output_traj) const; + + drake::systems::InputPortIndex franka_state_port_; + drake::systems::InputPortIndex object_state_port_; + drake::systems::OutputPortIndex lcs_state_port_; + + int num_end_effector_positions_; + int num_object_positions_; + int num_end_effector_velocities_; + int num_object_velocities_; + + const drake::multibody::MultibodyPlant& franka_plant_; + drake::systems::Context* franka_context_; + const drake::multibody::MultibodyPlant& object_plant_; + drake::systems::Context* object_context_; + const drake::multibody::Frame& world_; + std::string end_effector_name_; + std::string object_name_; + const bool include_end_effector_orientation_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/franka_kinematics_vector.cc b/examples/franka/systems/franka_kinematics_vector.cc new file mode 100644 index 0000000000..6dce76fe09 --- /dev/null +++ b/examples/franka/systems/franka_kinematics_vector.cc @@ -0,0 +1,6 @@ +#include "examples/franka/systems/franka_kinematics_vector.h" + +#include "drake/common/default_scalars.h" + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::dairlib::systems::FrankaKinematicsVector) diff --git a/examples/franka/systems/franka_kinematics_vector.h b/examples/franka/systems/franka_kinematics_vector.h new file mode 100644 index 0000000000..a3288c7d8e --- /dev/null +++ b/examples/franka/systems/franka_kinematics_vector.h @@ -0,0 +1,177 @@ +#pragma once + +#include +#include + +#include "systems/framework/timestamped_vector.h" + +namespace dairlib { +namespace systems { + +/// FrankaKinematicsVector stores the robot output as a TimestampedVector +/// * positions +/// * velocities +template +class FrankaKinematicsVector : public TimestampedVector { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaKinematicsVector) + + FrankaKinematicsVector() = default; + + explicit FrankaKinematicsVector(int num_end_effector_positions, + int num_object_positions, + int num_end_effector_velocities, + int num_object_velocities) + : TimestampedVector(num_end_effector_positions + num_object_positions + + num_end_effector_velocities + + num_object_velocities), + num_end_effector_positions_(num_end_effector_positions), + num_object_positions_(num_object_positions), + num_end_effector_velocities_(num_end_effector_velocities), + num_object_velocities_(num_object_velocities), + end_effector_positions_start_(0), + object_positions_start_(num_end_effector_positions_), + end_effector_velocities_start_(num_end_effector_positions_ + + num_object_positions_), + object_velocities_start_(num_end_effector_positions_ + + num_object_positions_ + + num_end_effector_velocities_), + num_positions_(num_end_effector_positions_ + num_object_positions_), + num_velocities_(num_end_effector_velocities_ + num_object_velocities_) { + } + + /// Constructs a OutputVector with the specified positions and velocities. + explicit FrankaKinematicsVector( + const drake::VectorX& end_effector_positions, + const drake::VectorX& object_positions, + const drake::VectorX& end_effector_velocities, + const drake::VectorX& object_velocities) + : FrankaKinematicsVector( + end_effector_positions.size(), object_positions.size(), + end_effector_velocities.size(), object_velocities.size()) { + this->SetEndEffectorPositions(end_effector_positions); + this->SetObjectPositions(object_positions); + this->SetEndEffectorVelocities(end_effector_velocities); + this->SetObjectVelocities(object_velocities); + } + + void SetEndEffectorPositions(drake::VectorX positions) { + DRAKE_DEMAND(positions.size() == num_end_effector_positions_); + this->get_mutable_data().segment(end_effector_positions_start_, + num_end_effector_positions_) = positions; + } + + void SetObjectPositions(drake::VectorX positions) { + DRAKE_DEMAND(positions.size() == num_object_positions_); + this->get_mutable_data().segment(object_positions_start_, + num_object_positions_) = positions; + } + + void SetEndEffectorVelocities(drake::VectorX velocities) { + DRAKE_DEMAND(velocities.size() == num_end_effector_velocities_); + this->get_mutable_data().segment(end_effector_velocities_start_, + num_end_effector_velocities_) = velocities; + } + + void SetObjectVelocities(drake::VectorX velocities) { + DRAKE_DEMAND(velocities.size() == num_object_velocities_); + this->get_mutable_data().segment(object_velocities_start_, + num_object_velocities_) = velocities; + } + + void SetState(drake::VectorX state) { + DRAKE_DEMAND(state.size() == this->data_size()); + this->get_mutable_data().segment(end_effector_positions_start_, + this->data_size()) = state; + } + + /// Returns a const state vector + const drake::VectorX GetState() const { + return this->get_data().segment(end_effector_positions_start_, + this->data_size()); + } + + /// Returns a const positions vector for the end effector + const drake::VectorX GetEndEffectorPositions() const { + return this->get_data().segment(end_effector_positions_start_, + num_end_effector_positions_); + } + + /// Returns a const positions vector for the object + const drake::VectorX GetObjectPositions() const { + return this->get_data().segment(object_positions_start_, + num_object_positions_); + } + + /// Returns a const positions vector for the end effector + const drake::VectorX GetEndEffectorVelocities() const { + return this->get_data().segment(end_effector_velocities_start_, + num_end_effector_velocities_); + } + + /// Returns a const positions vector for the object + const drake::VectorX GetObjectVelocities() const { + return this->get_data().segment(object_velocities_start_, + num_object_velocities_); + } + + /// Returns a const velocities vector + const drake::VectorX GetVelocities() const { + return this->get_data().segment( + end_effector_velocities_start_, + num_end_effector_velocities_ + num_object_velocities_); + } + + /// Returns a const positions vector + const drake::VectorX GetPositions() const { + return this->get_data().segment( + end_effector_positions_start_, + num_end_effector_positions_ + num_object_positions_); + } + + /// Returns a mutable positions vector + Eigen::Map> GetMutablePositions() { + auto data = this->get_mutable_data().segment( + end_effector_positions_start_, + num_end_effector_positions_ + num_object_positions_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable velocities vector + Eigen::Map> GetMutableVelocities() { + auto data = this->get_mutable_data().segment( + end_effector_velocities_start_, + num_end_effector_velocities_ + num_object_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable state vector + Eigen::Map> GetMutableState() { + auto data = this->get_mutable_data().segment(end_effector_positions_start_, + this->data_size()); + return Eigen::Map>(&data(0), data.size()); + } + + protected: + virtual FrankaKinematicsVector* DoClone() const { + return new FrankaKinematicsVector( + num_end_effector_positions_, num_object_positions_, + num_end_effector_velocities_, num_object_velocities_); + } + + private: + const int num_end_effector_positions_; + const int num_object_positions_; + const int num_end_effector_velocities_; + const int num_object_velocities_; + const int end_effector_positions_start_; + const int object_positions_start_; + const int end_effector_velocities_start_; + const int object_velocities_start_; + + const int num_positions_; + const int num_velocities_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/franka_state_translator.cc b/examples/franka/systems/franka_state_translator.cc new file mode 100644 index 0000000000..46277ddb52 --- /dev/null +++ b/examples/franka/systems/franka_state_translator.cc @@ -0,0 +1,82 @@ +#include "examples/franka/systems/franka_state_translator.h" + +#include + +#include "lcmtypes/drake/lcmt_panda_status.hpp" +#include "systems/framework/output_vector.h" + +namespace dairlib { + +using drake::systems::BasicVector; +using drake::systems::Context; +using systems::TimestampedVector; + +namespace systems { + +FrankaStateOutTranslator::FrankaStateOutTranslator( + std::vector joint_position_names, + std::vector joint_velocity_names, + std::vector joint_actuator_names) { + this->set_name("franka_state_translator"); + + panda_status_ = + this->DeclareAbstractInputPort("franka_state", + drake::Value()) + .get_index(); + DRAKE_DEMAND(joint_position_names.size() == kNumFrankaJoints); + DRAKE_DEMAND(joint_velocity_names.size() == kNumFrankaJoints); + DRAKE_DEMAND(joint_actuator_names.size() == kNumFrankaJoints); + auto default_robot_output_msg = dairlib::lcmt_robot_output(); + default_robot_output_msg.position_names = joint_position_names; + default_robot_output_msg.velocity_names = joint_velocity_names; + default_robot_output_msg.effort_names = joint_actuator_names; + state_output_ = this->DeclareAbstractOutputPort( + "lcmt_robot_output", default_robot_output_msg, + &FrankaStateOutTranslator::OutputFrankaState) + .get_index(); +} + +void FrankaStateOutTranslator::OutputFrankaState( + const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const { + const auto& panda_status = + this->EvalInputValue(context, panda_status_); + output->utime = panda_status->utime; + output->position = panda_status->joint_position; + output->num_positions = panda_status->num_joints; + output->num_velocities = panda_status->num_joints; + output->num_efforts = panda_status->num_joints; +// output->position_names = std::vector(output->num_positions, "double"); + output->velocity = panda_status->joint_velocity; + output->effort = panda_status->joint_torque; +} + +FrankaEffortsInTranslator::FrankaEffortsInTranslator() { + this->set_name("franka_command_translator"); + + robot_input_ = + this->DeclareAbstractInputPort("franka_state", + drake::Value()) + .get_index(); + + franka_command_output_ = + this->DeclareAbstractOutputPort( + "lcmt_panda_command", + &FrankaEffortsInTranslator::OutputFrankaCommand) + .get_index(); +} + +void FrankaEffortsInTranslator::OutputFrankaCommand( + const drake::systems::Context& context, + drake::lcmt_panda_command* output) const { + const auto& robot_input = + this->EvalInputValue(context, robot_input_); + DRAKE_DEMAND(robot_input->efforts.size() == kNumFrankaJoints); + output->utime = robot_input->utime; + output->num_joint_torque = robot_input->efforts.size(); + output->joint_torque = robot_input->efforts; + output->control_mode_expected = drake::lcmt_panda_status::CONTROL_MODE_TORQUE; +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/franka_state_translator.h b/examples/franka/systems/franka_state_translator.h new file mode 100644 index 0000000000..f1a3bc8c39 --- /dev/null +++ b/examples/franka/systems/franka_state_translator.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include + +#include "common/find_resource.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +const static int kNumFrankaJoints = 7; + +class FrankaStateOutTranslator : public drake::systems::LeafSystem { + public: + explicit FrankaStateOutTranslator(std::vector joint_position_names, + std::vector joint_velocity_names, + std::vector joint_actuator_names); + + const drake::systems::InputPort& get_input_port_panda_status() const { + return this->get_input_port(panda_status_); + } + const drake::systems::OutputPort& get_output_port_robot_state() + const { + return this->get_output_port(state_output_); + } + + private: + void OutputFrankaState(const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const; + + drake::systems::InputPortIndex panda_status_; + drake::systems::OutputPortIndex state_output_; +}; + +class FrankaEffortsInTranslator : public drake::systems::LeafSystem { + public: + explicit FrankaEffortsInTranslator(); + + const drake::systems::InputPort& get_input_port_efforts() const { + return this->get_input_port(robot_input_); + } + const drake::systems::OutputPort& get_output_port_panda_command() + const { + return this->get_output_port(franka_command_output_); + } + + private: + void OutputFrankaCommand(const drake::systems::Context& context, + drake::lcmt_panda_command* output) const; + + drake::systems::InputPortIndex robot_input_; + drake::systems::OutputPortIndex franka_command_output_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/plate_balancing_target.cc b/examples/franka/systems/plate_balancing_target.cc new file mode 100644 index 0000000000..a503d18af1 --- /dev/null +++ b/examples/franka/systems/plate_balancing_target.cc @@ -0,0 +1,199 @@ +#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 new file mode 100644 index 0000000000..be73d052b1 --- /dev/null +++ b/examples/franka/systems/plate_balancing_target.h @@ -0,0 +1,81 @@ +#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 new file mode 100644 index 0000000000..90ace69be8 --- /dev/null +++ b/examples/franka/urdf/center_support.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + cod + + + + + + + + + + + cod + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector.urdf b/examples/franka/urdf/plate_end_effector.urdf new file mode 100644 index 0000000000..a0efe0cef0 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector_massless.urdf b/examples/franka/urdf/plate_end_effector_massless.urdf new file mode 100644 index 0000000000..b81d96de63 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_massless.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector_translation.urdf b/examples/franka/urdf/plate_end_effector_translation.urdf new file mode 100644 index 0000000000..bb2ca2f4f8 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_translation.urdf @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + 1 + + + + + 1 + + diff --git a/examples/franka/urdf/side_wall.urdf b/examples/franka/urdf/side_wall.urdf new file mode 100644 index 0000000000..4cb74faec7 --- /dev/null +++ b/examples/franka/urdf/side_wall.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + cod + + + + + + + + + + + cod + + + + + + + diff --git a/examples/franka/urdf/support.urdf b/examples/franka/urdf/support.urdf new file mode 100644 index 0000000000..0bfd702a4f --- /dev/null +++ b/examples/franka/urdf/support.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/support_point_contact.urdf b/examples/franka/urdf/support_point_contact.urdf new file mode 100644 index 0000000000..3e06df6c64 --- /dev/null +++ b/examples/franka/urdf/support_point_contact.urdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/tray.sdf b/examples/franka/urdf/tray.sdf new file mode 100644 index 0000000000..b774f23a7b --- /dev/null +++ b/examples/franka/urdf/tray.sdf @@ -0,0 +1,49 @@ + + + + + + 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 new file mode 100644 index 0000000000..0f5862fe11 --- /dev/null +++ b/examples/franka/urdf/tray_lcs.sdf @@ -0,0 +1,43 @@ + + + + + + 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/lcm/lcm_trajectory.cc b/lcm/lcm_trajectory.cc index dec3b50dd7..7508bbafe3 100644 --- a/lcm/lcm_trajectory.cc +++ b/lcm/lcm_trajectory.cc @@ -38,7 +38,7 @@ std::string exec(const char* cmd) { return result; } -LcmTrajectory::Trajectory::Trajectory(string traj_name, +LcmTrajectory::Trajectory::Trajectory(const string& traj_name, const lcmt_trajectory_block& traj_block) { int num_points = traj_block.num_points; int num_datatypes = traj_block.num_datatypes; @@ -47,6 +47,7 @@ LcmTrajectory::Trajectory::Trajectory(string traj_name, this->time_vector = VectorXd::Map(traj_block.time_vec.data(), num_points); this->datapoints = MatrixXd(num_datatypes, num_points); + DRAKE_ASSERT(traj_block.datatypes.size() == traj_block.datapoints.size()); // Convert vector> to EigenMatrix for (int i = 0; i < num_datatypes; ++i) { this->datapoints.row(i) = @@ -63,6 +64,7 @@ LcmTrajectory::LcmTrajectory(const vector& trajectories, for (const string& traj_name : trajectory_names_) { trajectories_[traj_name] = trajectories[index++]; } + metadata_.git_dirty_flag = false; if (get_metadata) { std::cout << "NOTE: Using subprocesses to get LcmTrajectory metadata\n"; ConstructMetadataObject(name, description); diff --git a/lcm/lcm_trajectory.h b/lcm/lcm_trajectory.h index 69d955b326..0eccb7de14 100644 --- a/lcm/lcm_trajectory.h +++ b/lcm/lcm_trajectory.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -26,7 +27,8 @@ class LcmTrajectory { /// lcmt_trajectory_block is the lcmtype analog struct Trajectory { Trajectory() = default; - Trajectory(std::string traj_name, const lcmt_trajectory_block& traj_block); + Trajectory(const std::string& traj_name, + const lcmt_trajectory_block& traj_block); std::string traj_name; Eigen::VectorXd time_vector; @@ -39,9 +41,8 @@ class LcmTrajectory { LcmTrajectory() = default; LcmTrajectory(const std::vector& trajectories, const std::vector& trajectory_names, - const std::string& name, - const std::string& description, - bool get_metadata=true); + const std::string& name, const std::string& description, + bool get_metadata = false); explicit LcmTrajectory(const lcmt_saved_traj& traj); @@ -64,8 +65,18 @@ class LcmTrajectory { lcmt_metadata GetMetadata() const { return metadata_; } + bool HasTrajectory(const std::string& trajectory_name) const { + return trajectories_.count(trajectory_name) > 0; + } + const Trajectory& GetTrajectory(const std::string& trajectory_name) const { - return trajectories_.at(trajectory_name); + try { + return trajectories_.at(trajectory_name); + } catch (std::exception& e) { + std::cerr << "Trajectory: " << trajectory_name << " does not exist." + << std::endl; + throw std::out_of_range(""); + } } /// Add additional LcmTrajectory::Trajectory objects @@ -76,8 +87,8 @@ class LcmTrajectory { const std::vector& GetTrajectoryNames() const { return trajectory_names_; } - lcmt_saved_traj GenerateLcmObject() const; + protected: /// Constructs a lcmt_metadata object with a specified name and description /// Other relevant metadata details such as datatime and git status are @@ -85,7 +96,6 @@ class LcmTrajectory { void ConstructMetadataObject(std::string name, std::string description); private: - lcmt_metadata metadata_; std::unordered_map trajectories_; std::vector trajectory_names_; diff --git a/lcmtypes/lcmt_c3_forces.lcm b/lcmtypes/lcmt_c3_forces.lcm new file mode 100644 index 0000000000..f9122bd72a --- /dev/null +++ b/lcmtypes/lcmt_c3_forces.lcm @@ -0,0 +1,11 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_c3_forces +{ + int64_t utime; + + int32_t num_forces; + lcmt_force forces[num_forces]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_intermediates.lcm b/lcmtypes/lcmt_c3_intermediates.lcm new file mode 100644 index 0000000000..c709decfb9 --- /dev/null +++ b/lcmtypes/lcmt_c3_intermediates.lcm @@ -0,0 +1,12 @@ +package dairlib; + +struct lcmt_c3_intermediates +{ + int32_t num_points; + int32_t num_total_variables; + + float time_vec[num_points]; + float z_sol[num_total_variables][num_points]; + float delta_sol[num_total_variables][num_points]; + float w_sol[num_total_variables][num_points]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_output.lcm b/lcmtypes/lcmt_c3_output.lcm new file mode 100644 index 0000000000..b82b674801 --- /dev/null +++ b/lcmtypes/lcmt_c3_output.lcm @@ -0,0 +1,9 @@ +package dairlib; + +struct lcmt_c3_output +{ + int64_t utime; + + lcmt_c3_solution c3_solution; + lcmt_c3_intermediates c3_intermediates; +} diff --git a/lcmtypes/lcmt_c3_solution.lcm b/lcmtypes/lcmt_c3_solution.lcm new file mode 100644 index 0000000000..9648477032 --- /dev/null +++ b/lcmtypes/lcmt_c3_solution.lcm @@ -0,0 +1,14 @@ +package dairlib; + +struct lcmt_c3_solution +{ + int32_t num_points; + int32_t num_state_variables; + int32_t num_contact_variables; + int32_t num_input_variables; + + float time_vec[num_points]; + float x_sol[num_state_variables][num_points]; + float lambda_sol[num_contact_variables][num_points]; + float u_sol[num_input_variables][num_points]; +} diff --git a/lcmtypes/lcmt_c3_state.lcm b/lcmtypes/lcmt_c3_state.lcm new file mode 100644 index 0000000000..1d1f4a2101 --- /dev/null +++ b/lcmtypes/lcmt_c3_state.lcm @@ -0,0 +1,10 @@ +package dairlib; + +struct lcmt_c3_state +{ + int64_t utime; + int32_t num_states; + + float state [num_states]; + string state_names [num_states]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_force.lcm b/lcmtypes/lcmt_force.lcm new file mode 100644 index 0000000000..935c9635aa --- /dev/null +++ b/lcmtypes/lcmt_force.lcm @@ -0,0 +1,12 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_force +{ + int64_t utime; + + // These are all expressed in the world frame. + double contact_point[3]; + double contact_force[3]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_object_state.lcm b/lcmtypes/lcmt_object_state.lcm new file mode 100644 index 0000000000..aa5d1cffa2 --- /dev/null +++ b/lcmtypes/lcmt_object_state.lcm @@ -0,0 +1,16 @@ +package dairlib; + +struct lcmt_object_state +{ + int64_t utime; + string object_name; + + int32_t num_positions; + int32_t num_velocities; + + string position_names [num_positions]; + double position [num_positions]; + + string velocity_names [num_velocities]; + double velocity [num_velocities]; +} diff --git a/lcmtypes/lcmt_timestamped_saved_traj.lcm b/lcmtypes/lcmt_timestamped_saved_traj.lcm new file mode 100644 index 0000000000..7a49bd7c2b --- /dev/null +++ b/lcmtypes/lcmt_timestamped_saved_traj.lcm @@ -0,0 +1,10 @@ +package dairlib; + +/* lcmtype analog for LcmTrajectory WITH TIMESTAMP to enable using a + LcmSerializer to save/load trajectories +*/ + +struct lcmt_timestamped_saved_traj { + int64_t utime; + lcmt_saved_traj saved_traj; +} diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index 60d79fe659..c8c023a068 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -1,17 +1,17 @@ #include "multibody/geom_geom_collider.h" -#include "multibody/geom_geom_collider.h" #include "drake/math/rotation_matrix.h" -using Eigen::Vector3d; -using Eigen::Matrix; using drake::EigenPtr; using drake::MatrixX; +using drake::VectorX; using drake::geometry::GeometryId; using drake::geometry::SignedDistancePair; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; using drake::systems::Context; +using Eigen::Matrix; +using Eigen::Vector3d; namespace dairlib { namespace multibody { @@ -22,8 +22,7 @@ GeomGeomCollider::GeomGeomCollider( const drake::SortedPair geometry_pair) : plant_(plant), geometry_id_A_(geometry_pair.first()), - geometry_id_B_(geometry_pair.second()) { -} + geometry_id_B_(geometry_pair.second()) {} template std::pair> GeomGeomCollider::Eval(const Context& context, @@ -35,22 +34,21 @@ template std::pair> GeomGeomCollider::EvalPolytope( const Context& context, int num_friction_directions, JacobianWrtVariable wrt) { - if (num_friction_directions == 1) { throw std::runtime_error( - "GeomGeomCollider cannot specificy 1 friction direction unless " - "using EvalPlanar."); + "GeomGeomCollider cannot specify 1 friction direction unless " + "using EvalPlanar."); } // Build friction basis - Matrix force_basis( - 2 * num_friction_directions + 1, 3); + Matrix force_basis(2 * num_friction_directions + 1, + 3); force_basis.row(0) << 1, 0, 0; for (int i = 0; i < num_friction_directions; i++) { double theta = (M_PI * i) / num_friction_directions; - force_basis.row(2*i + 1) = Vector3d(0, cos(theta), sin(theta)); - force_basis.row(2*i + 2) = -force_basis.row(2*i + 1); + force_basis.row(2 * i + 1) = Vector3d(0, cos(theta), sin(theta)); + force_basis.row(2 * i + 2) = -force_basis.row(2 * i + 1); } return DoEval(context, force_basis, wrt); } @@ -62,12 +60,41 @@ std::pair> GeomGeomCollider::EvalPlanar( return DoEval(context, planar_normal.transpose(), wrt, true); } - // } +template +std::pair, VectorX> +GeomGeomCollider::CalcWitnessPoints(const Context& context) { + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_, + geometry_id_B_); + const auto& inspector = query_object.inspector(); + const auto frame_A_id = inspector.GetFrameId(geometry_id_A_); + const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); + const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); + const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); + + const Vector3d& p_ACa = + inspector.GetPoseInFrame(geometry_id_A_).template cast() * + signed_distance_pair.p_ACa; + const Vector3d& p_BCb = + inspector.GetPoseInFrame(geometry_id_B_).template cast() * + signed_distance_pair.p_BCb; + Vector3d p_WCa = Vector3d::Zero(); + Vector3d p_WCb = Vector3d::Zero(); + plant_.CalcPointsPositions(context, frameA, p_ACa, plant_.world_frame(), + &p_WCa); + plant_.CalcPointsPositions(context, frameB, p_BCb, plant_.world_frame(), + &p_WCb); + return std::pair, VectorX>(p_WCa, p_WCb); +} + template std::pair> GeomGeomCollider::DoEval( const Context& context, Matrix force_basis, JacobianWrtVariable wrt, bool planar) { - const auto& query_port = plant_.get_geometry_query_input_port(); const auto& query_object = query_port.template Eval>(context); @@ -84,27 +111,25 @@ std::pair> GeomGeomCollider::DoEval( const Vector3d& p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * - signed_distance_pair.p_ACa; + signed_distance_pair.p_ACa; const Vector3d& p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * - signed_distance_pair.p_BCb; - + signed_distance_pair.p_BCb; - int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() : - plant_.num_positions(); + int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() + : plant_.num_positions(); Matrix Jv_WCa(3, n_cols); Matrix Jv_WCb(3, n_cols); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameA, p_ACa, plant_.world_frame(), - plant_.world_frame(), &Jv_WCa); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameB, p_BCb, plant_.world_frame(), - plant_.world_frame(), &Jv_WCb); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameA, p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCa); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameB, p_BCb, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCb); - const auto& R_WC = - drake::math::RotationMatrix::MakeFromOneVector( - signed_distance_pair.nhat_BA_W, 0); + const auto& R_WC = drake::math::RotationMatrix::MakeFromOneVector( + signed_distance_pair.nhat_BA_W, 0); // if this is a planar problem, then the basis has one row and encodes // the planar normal direction. @@ -116,7 +141,8 @@ std::pair> GeomGeomCollider::DoEval( force_basis.resize(3, 3); // First row is the contact normal, projected to the plane - force_basis.row(0) = signed_distance_pair.nhat_BA_W - + force_basis.row(0) = + signed_distance_pair.nhat_BA_W - planar_normal * planar_normal.dot(signed_distance_pair.nhat_BA_W); force_basis.row(0).normalize(); @@ -125,7 +151,7 @@ std::pair> GeomGeomCollider::DoEval( force_basis.row(1).normalize(); force_basis.row(2) = -force_basis.row(1); } - // Standard case + // Standard case auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb); return std::pair>(signed_distance_pair.distance, J); } @@ -133,5 +159,4 @@ std::pair> GeomGeomCollider::DoEval( } // namespace multibody } // namespace dairlib - template class dairlib::multibody::GeomGeomCollider; \ No newline at end of file diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 5e32b95290..9d3b107f5a 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -31,7 +31,6 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian. /// Jacobian is ordered [J_n; J_t], and has shape //// (2*num_friction_directions + 1) x (nq or nv), depending @@ -48,12 +47,10 @@ class GeomGeomCollider { /// @param num_friction_directions /// @return A pair with std::pair> EvalPolytope( - const drake::systems::Context& context, - int num_friction_directions, + const drake::systems::Context& context, int num_friction_directions, drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian for a 2D planar problem /// Jacobian is ordered [J_n; +J_t; -J_t], and has shape 3 x (nq). /// J_t refers to the (contact_normal x planar_normal) direction @@ -66,6 +63,9 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); + std::pair, drake::VectorX> CalcWitnessPoints( + const drake::systems::Context& context); + private: std::pair> DoEval( const drake::systems::Context& context, diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 638e9a6106..d558dbd5c2 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -19,6 +19,64 @@ cc_library( ], ) +cc_library( + name = "c3", + srcs = [ + "c3.cc", + "c3_miqp.cc", + "c3_qp.cc", + ], + hdrs = [ + "c3.h", + "c3_miqp.h", + "c3_options.h", + "c3_qp.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/c3.cc b/solvers/c3.cc new file mode 100644 index 0000000000..2e2aeb2615 --- /dev/null +++ b/solvers/c3.cc @@ -0,0 +1,465 @@ +#include "solvers/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; + +C3::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; +} + +C3::C3(const LCS& lcs, const C3::CostMatrices& costs, + const vector& x_desired, const C3Options& options) + : warm_start_(options.warm_start), + 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_desired), + options_(options), + h_is_zero_(lcs.H_[0].isZero(0)), + prog_(MathematicalProgram()), + osqp_(OsqpSolver()) { + if (warm_start_) { + 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(n_ + m_ + k_); + } + 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_); + } + } + } + + 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_; + } + + 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(n_ + m_ + k_)); + 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(n_ + m_ + k_)); + delta_sol_->push_back(Eigen::VectorXd::Zero(n_ + m_ + k_)); + } + + 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))); + } + } + + MatrixXd LinEq(n_, 2 * n_ + m_ + k_); + LinEq.block(0, n_ + m_ + k_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); + dynamics_constraints_.resize(N_); + target_cost_.resize(N_ + 1); + for (int i = 0; i < N_; i++) { + LinEq.block(0, 0, n_, n_) = lcs.A_.at(i); + LinEq.block(0, n_, n_, m_) = lcs.D_.at(i); + LinEq.block(0, n_ + m_, n_, k_) = lcs.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(); + } + 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 C3::UpdateLCS(const 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); + + MatrixXd LinEq = MatrixXd::Zero(n_, 2 * n_ + m_ + k_); + LinEq.block(0, n_ + k_ + m_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); + auto Dn = D_[0].norm(); + auto An = A_[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_; + } + + 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, -lcs.d_.at(i)); + } +} + +void C3::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 C3::Solve(const VectorXd& x0) { + auto start = std::chrono::high_resolution_clock::now(); + + VectorXd delta_init = VectorXd::Zero(n_ + m_ + k_); + if (options_.delta_option == 1) { + delta_init.head(n_) = x0; + } + std::vector delta(N_, delta_init); + std::vector w(N_, VectorXd::Zero(n_ + m_ + k_)); + vector Gv = G_; + + for (int i = 0; i < N_; ++i) { + input_costs_[i]->UpdateCoefficients(2 * R_.at(i), + -2 * R_.at(i) * u_sol_->at(i)); + } + + for (int iter = 0; iter < options_.admm_iter; iter++) { + ADMMStep(x0, &delta, &w, &Gv, iter); + } + + vector WD(N_, VectorXd::Zero(n_ + m_ + k_)); + for (int i = 0; i < N_; i++) { + WD.at(i) = delta.at(i) - w.at(i); + } + + vector zfin = SolveQP(x0, Gv, WD, options_.admm_iter, true); + + *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_; + } + + auto finish = std::chrono::high_resolution_clock::now(); + auto elapsed = finish - start; + solve_time_ = + std::chrono::duration_cast(elapsed).count() / + 1e6; +} + +void C3::ADMMStep(const VectorXd& x0, vector* delta, + vector* w, vector* Gv, + int admm_iteration) { + vector WD(N_, VectorXd::Zero(n_ + m_ + k_)); + + for (int i = 0; i < N_; i++) { + WD.at(i) = delta->at(i) - w->at(i); + } + + vector z = SolveQP(x0, *Gv, WD, admm_iteration, true); + + vector ZW(N_, VectorXd::Zero(n_ + m_ + k_)); + 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); + } + + 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; + } +} + +vector C3::SolveQP(const VectorXd& x0, const vector& G, + const vector& WD, int admm_iteration, + bool is_final_solve) { + for (auto& constraint : constraints_) { + prog_.RemoveConstraint(constraint); + } + constraints_.clear(); + constraints_.push_back(prog_.AddLinearConstraint(x_[0] == x0)); + + if (h_is_zero_ == 1) { // No dependence on u, so just simulate passive system + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd lambda0; + LCPSolver.SolveLcpLemke(F_[0], E_[0] * x0 + c_[0], &lambda0); + constraints_.push_back(prog_.AddLinearConstraint(lambda_[0] == lambda0)); + } + + for (auto& cost : costs_) { + prog_.RemoveCost(cost); + } + costs_.clear(); + + for (int i = 0; i < N_ + 1; i++) { + if (i < N_) { + 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_, 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_, 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)); + } + } + + // /// initialize decision variables to warm start + 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_]); + } + + MathematicalProgramResult result = osqp_.Solve(prog_); + + if (result.is_success()) { + 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]); + + if (warm_start_) { + // update warm start parameters + 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]); + } + } + if (warm_start_) { + warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); + } + } + + return *z_sol_; +} + +vector C3::SolveProjection(const vector& G, + vector& WZ, int admm_iteration) { + vector deltaProj(N_, VectorXd::Zero(n_ + m_ + k_)); + 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 + omp_set_nested(1); + } + +#pragma omp parallel for num_threads(options_.num_threads) + for (i = 0; i < N_; i++) { + if (options_.use_robust_formulation && + admm_iteration == + (options_.admm_iter - 1)) { // only on the last iteration + if (warm_start_) { + if (i == N_ - 1) { + deltaProj[i] = SolveRobustSingleProjection( + G[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, -1); + } else { + deltaProj[i] = SolveRobustSingleProjection( + G[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, i + 1); + } + } else { + deltaProj[i] = SolveRobustSingleProjection( + G[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, -1); + } + } else { + if (warm_start_) { + if (i == N_ - 1) { + deltaProj[i] = SolveSingleProjection(G[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, -1); + } else { + deltaProj[i] = SolveSingleProjection(G[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, i + 1); + } + } else { + deltaProj[i] = SolveSingleProjection(G[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, -1); + } + } + } + + return deltaProj; +} + +void C3::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 C3::RemoveConstraints() { + for (auto& userconstraint : user_constraints_) { + prog_.RemoveConstraint(userconstraint); + } + user_constraints_.clear(); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3.h b/solvers/c3.h new file mode 100644 index 0000000000..e68c606c9e --- /dev/null +++ b/solvers/c3.h @@ -0,0 +1,184 @@ +#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 C3 { + 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 LCS parameters + /// @param Q, R, G, U Cost function parameters + C3(const LCS& LCS, const CostMatrices& costs, + const std::vector& x_desired, const C3Options& options); + + virtual ~C3() = default; + + /// Solve the MPC problem + /// @param x0 The initial state of the system + /// @param delta A pointer to the copy variable solution + /// @param w A pointer to the scaled dual variable solution + /// @return The first control action to take, u[0] + void Solve(const Eigen::VectorXd& x0); + + /// 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 + void ADMMStep(const Eigen::VectorXd& x0, std::vector* delta, + std::vector* w, + std::vector* G, int admm_iteration); + + /// Solve a single QP + /// @param x0 The initial state of the system + /// @param WD A pointer to the (w - delta) variables + /// @param G A pointer to the G variables from previous step + std::vector SolveQP(const Eigen::VectorXd& x0, + const std::vector& G, + const std::vector& WD, + int admm_iteration, + bool is_final_solve = false); + + /// Solve the projection problem for all timesteps + /// @param WZ A pointer to the (z + w) variables + /// @param G A pointer to the G variables from previous step + std::vector SolveProjection( + const std::vector& G, 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); + + /// allow user to remove all constraints + void RemoveConstraints(); + + /// Solve a single projection step + /// @param E, F, H, c LCS parameters + /// @param U A pointer to the U variables + /// @param delta_c A pointer to the copy of (z + w) variables + 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; + + /// Solve a robust single projection step + /// @param E, F, H, c LCS parameters + /// @param U A pointer to the U variables + /// @param delta_c A pointer to the copy of (z + w) variables + virtual Eigen::VectorXd SolveRobustSingleProjection( + 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 Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + 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_; } + + public: + void UpdateLCS(const LCS& lcs); + void UpdateTarget(const std::vector& x_des); + + protected: + 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 + + private: + 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; + const std::vector Q_; + const std::vector R_; + const std::vector U_; + const std::vector G_; + std::vector x_desired_; + const C3Options options_; + double dt_ = 0; + double solve_time_ = 0; + bool h_is_zero_; + + drake::solvers::MathematicalProgram prog_; + // QP step variables + drake::solvers::OsqpSolver osqp_; + std::vector x_; + std::vector u_; + std::vector lambda_; + // QP step constraints + std::vector dynamics_constraints_; + std::vector> + constraints_; + std::vector> + user_constraints_; + + /// Projection step variables are defined outside of the MathematicalProgram + /// interface + + std::vector target_cost_; + std::vector> costs_; + std::vector> input_costs_; + + // Solutions + 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 new file mode 100644 index 0000000000..1d1e4d3a37 --- /dev/null +++ b/solvers/c3_miqp.cc @@ -0,0 +1,295 @@ +#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) + : C3(LCS, costs, xdesired, options), env_(true) { + // Create an environment + // env_.set("LogToConsole", "0"); + env_.set("OutputFlag", "0"); + env_.set("Threads", "0"); + 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; + + // 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); + + // initial state constraint +// if (warm_start_index == 0){ +// for (int i = 0; i < n_; ++i){ +// model.addConstr(delta_k[i] == delta_c[i]); +// } +// } + + int M = 1000; // 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]); + } + + model.optimize(); + + 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; +} + +VectorXd C3MIQP::SolveRobustSingleProjection( + const MatrixXd& U, const VectorXd& delta_c, const MatrixXd& E, + const MatrixXd& F, const MatrixXd& H, const VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index) { + // set up linear term in cost + VectorXd cost_lin = -2 * delta_c.transpose() * U; + + // 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_]; + GRBVar reduced_friction_cone_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]); + } + + // initial state constraint + if (warm_start_index == 0){ + for (int i = 0; i < n_; ++i){ + model.addConstr(delta_k[i] == delta_c[i]); + } + } + + + model.setObjective(obj, GRB_MINIMIZE); + + int M = 1000; // 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]); + } + + double* tangential_velocity_coeffs; + MatrixXd P_t(m_, n_ + m_ + k_); + int constraint_rows = m_; + P_t << W_x, W_l, W_u; + + // stewart and trinkle +// P_t << E, F, H; +// MatrixXd P_t2 = P_t.bottomRows(((m_ / 6) * 4) / 7 * 3); +// P_t = P_t2; +// constraint_rows = P_t2.rows(); + + for (int i = 0; i < constraint_rows; ++i) { + reduced_friction_cone_binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + GRBLinExpr tangential_velocity_expr = 0; + tangential_velocity_coeffs = P_t.row(i).data(); + tangential_velocity_expr.addTerms(tangential_velocity_coeffs, delta_k, + n_ + m_ + k_); + + /// Constraint explanation: + /// Adding a binary decision to either be: + /// - inside a conservative (mu_l) friction cone (stick) + /// or + /// - on the boundary of the original friction cone (mu) (slip) + /// where mu_l < mu + + /// The active constraint for the sticking condition is: + /// lambda_1 - lambda_2 <= mu_l / mu * (lambda_1 + lambda_2) + + /// The active constraint for the friction cone boundary is: + /// lambda_2 == 0 + /// where lambda_2 is the friction force in the sliding direction + /// tangential velocity expr is the tangential velocity in the SAME direction as the friction force + if (i % 2 == 1) { + model.addConstr(delta_k[n_ + i - 1] - delta_k[n_ + i] <= + (0.5 / 0.6) * (delta_k[n_ + i] + delta_k[n_ + i - 1]) + + M * (reduced_friction_cone_binary[i])); + model.addConstr(tangential_velocity_expr <= + M * (reduced_friction_cone_binary[i])); + model.addConstr(delta_k[n_ + i] <= + M * (1 - reduced_friction_cone_binary[i])); + } else { + model.addConstr(delta_k[n_ + i + 1] - delta_k[n_ + i] <= + (0.5 / 0.6) * (delta_k[n_ + i] + delta_k[n_ + i + 1]) + + M * (reduced_friction_cone_binary[i])); + model.addConstr(tangential_velocity_expr <= + M * (reduced_friction_cone_binary[i])); + model.addConstr(delta_k[n_ + i] <= + M * (1 - reduced_friction_cone_binary[i])); + } + /// if i % 2 == 0: + /// lambda_1 = 0 + /// else: + /// lambda_2 = 0 +// model.addConstr(delta_k[n_ + i] <= +// M * (1 - reduced_friction_cone_binary[i])); + } + + model.optimize(); + + 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 diff --git a/solvers/c3_miqp.h b/solvers/c3_miqp.h new file mode 100644 index 0000000000..0a80f70148 --- /dev/null +++ b/solvers/c3_miqp.h @@ -0,0 +1,47 @@ +#pragma once + +#include + +#include + +#include "gurobi_c++.h" +#include "solvers/c3.h" +#include "solvers/lcs.h" + +namespace dairlib { +namespace solvers { + +class C3MIQP final : public C3 { + 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; + /// Robust projection method + Eigen::VectorXd SolveRobustSingleProjection( + 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 Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + 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 new file mode 100644 index 0000000000..6b201063fb --- /dev/null +++ b/solvers/c3_options.h @@ -0,0 +1,159 @@ +#pragma once +#include + +#include "drake/common/yaml/yaml_read_archive.h" + +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; + bool use_robust_formulation; + double solve_time_filter_alpha; + double publish_frequency; + +// std::vector world_x_limits; +// std::vector world_y_limits; +// std::vector world_z_limits; + 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_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_u; + + std::vector mu; + double dt; + double solve_dt; + 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(use_robust_formulation)); + 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(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)); + + 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()); + 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()); + + 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 new file mode 100644 index 0000000000..2f68be4b3d --- /dev/null +++ b/solvers/c3_output.cc @@ -0,0 +1,81 @@ +#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 new file mode 100644 index 0000000000..6e4f574211 --- /dev/null +++ b/solvers/c3_output.h @@ -0,0 +1,52 @@ +#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_qp.cc b/solvers/c3_qp.cc new file mode 100644 index 0000000000..1fbfa9af95 --- /dev/null +++ b/solvers/c3_qp.cc @@ -0,0 +1,116 @@ +#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) + : C3(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"); + + double alpha = 0.01; + double scaling = 1000; + + MatrixXd EFH(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(New_U, cost_linear, {xn_, ln_, un_}, 1); + + // prog.AddQuadraticCost((1 - alpha) * F, VectorXd::Zero(m_), ln_, 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); + solver_options.SetOption(OsqpSolver::id(), "linsys_solver", 0); + 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; +} + +VectorXd C3QP::SolveRobustSingleProjection(const MatrixXd& U, const VectorXd& delta_c, const MatrixXd& E, + const MatrixXd& F, const MatrixXd& H, const VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index) { + return delta_c; +} + +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 new file mode 100644 index 0000000000..dee6c58c2c --- /dev/null +++ b/solvers/c3_qp.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include + +#include "gurobi_c++.h" +#include "solvers/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 C3 { + 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; + /// Robust projection method + Eigen::VectorXd SolveRobustSingleProjection( + 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 Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + 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 new file mode 100644 index 0000000000..7433279a38 --- /dev/null +++ b/solvers/lcs.cc @@ -0,0 +1,113 @@ +#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(VectorXd& x_init, VectorXd& input) { + VectorXd x_final; + // calculate force + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd force; + + auto flag = LCPSolver.SolveLcpLemke( + F_[0], E_[0] * x_init + c_[0] + H_[0] * input, &force); + // update + x_final = A_[0] * x_init + B_[0] * input + D_[0] * force + d_[0]; + return x_final; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs.h b/solvers/lcs.h new file mode 100644 index 0000000000..9cfe7b85ce --- /dev/null +++ b/solvers/lcs.h @@ -0,0 +1,81 @@ +#pragma once +#include +#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(Eigen::VectorXd& x_init, + Eigen::VectorXd& input); + + 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 new file mode 100644 index 0000000000..d979afb2d8 --- /dev/null +++ b/solvers/lcs_factory.cc @@ -0,0 +1,449 @@ +#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, + int num_friction_directions, const std::vector& mu, double dt, + int N, 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()); + 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(2 * n_contacts * num_friction_directions, n_v); + + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider( + plant, + contact_geoms[i]); // deleted num_friction_directions (check with + // Michael about changes in geomgeom) + auto [phi_i, J_i] = collider.EvalPolytope(context, num_friction_directions); + // J_i is 3 x n_v + // row (0) is contact normal + // rows (1-num_friction directions) are the contact tangents + + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + 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(n_x, n_x); + MatrixXd B(n_x, n_u); + VectorXd d(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, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + + int n_lambda = 0; + if (contact_model == ContactModel::kStewartAndTrinkle) { + n_lambda = 2 * n_contacts + 2 * n_contacts * num_friction_directions; + } else { + n_lambda = 2 * n_contacts * num_friction_directions; + } + + // 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, 2 * n_contacts * num_friction_directions) = + dt * dt * qdotNv * MinvJ_t_T; + D.block(n_q, 2 * n_contacts, n_v, + 2 * n_contacts * num_friction_directions) = 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, 2 * n_contacts * num_friction_directions, 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, 2 * n_contacts * num_friction_directions, + 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, + 2 * n_contacts * num_friction_directions) = -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, + 2 * n_contacts * num_friction_directions) = + 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, 2 * n_contacts * num_friction_directions, + 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, + 2 * n_contacts * num_friction_directions, 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, + 2 * n_contacts * num_friction_directions, + 2 * n_contacts * num_friction_directions) = 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, 2 * n_contacts * num_friction_directions, 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, 2 * n_contacts * num_friction_directions) = + 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((2 * num_friction_directions) * i, + 2 * num_friction_directions) = + mu_vec(i) * VectorXd::Ones(2 * num_friction_directions); + } + 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, + int num_friction_directions, const std::vector& mu, + 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(2 * n_contacts * num_friction_directions, n_v); + std::vector contact_points; + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider( + plant, + contact_geoms[i]); // deleted num_friction_directions (check with + // Michael about changes in geomgeom) + auto [phi_i, J_i] = collider.EvalPolytope(context, num_friction_directions); + 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(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + if (contact_model == ContactModel::kStewartAndTrinkle) { + MatrixXd J_c = MatrixXd::Zero( + n_contacts + 2 * n_contacts * num_friction_directions, 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, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + int n_contact_vars = 2 * n_contacts * num_friction_directions; + + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + VectorXd anitescu_mu_vec = VectorXd::Zero(n_contact_vars); + for (int i = 0; i < mu_vec.rows(); i++) { + double cur = mu_vec(i); + anitescu_mu_vec(4 * i) = cur; + anitescu_mu_vec(4 * i + 1) = cur; + anitescu_mu_vec(4 * i + 2) = cur; + anitescu_mu_vec(4 * i + 3) = cur; + } + 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_); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs_factory.h b/solvers/lcs_factory.h new file mode 100644 index 0000000000..e86d94d439 --- /dev/null +++ b/solvers/lcs_factory.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include "solvers/lcs.h" + +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, + int num_friction_directions, const std::vector& mu, double dt, + int N, ContactModel = ContactModel::kStewartAndTrinkle); + + static std::pair> ComputeContactJacobian( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, + 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); +}; + +} // namespace solvers +} // namespace dairlib diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index d760c8b3b7..5b9c8a89c2 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -2,7 +2,6 @@ load("@drake//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) - cc_library( name = "controllers_all", srcs = [], @@ -10,6 +9,15 @@ cc_library( ":affine_controller", ":controller_failure_aggregator", ":fsm_event_time", + ":gravity_compensator", + ], +) + +cc_library( + name = "c3_systems", + deps = [ + ":c3_controller", + ":lcs_factory_system", ], ) @@ -40,6 +48,48 @@ 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 = "gravity_compensator", + srcs = [ + "gravity_compensator.cc", + ], + hdrs = [ + "gravity_compensator.h", + ], + deps = [ + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "linear_controller", srcs = [ @@ -185,36 +235,3 @@ cc_library( "@drake//:drake_shared_library", ], ) - -cc_library( - name = "safe_velocity_controller", - srcs = ["safe_velocity_controller.cc"], - hdrs = ["safe_velocity_controller.h"], - deps = [ - "//common", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "endeffector_position_controller", - srcs = ["endeffector_position_controller.cc"], - hdrs = ["endeffector_position_controller.h"], - deps = [ - "//common", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "endeffector_velocity_controller", - srcs = ["endeffector_velocity_controller.cc"], - hdrs = ["endeffector_velocity_controller.h"], - deps = [ - "//common", - "//systems/framework:vector", - "@drake//:drake_shared_library", - ], -) diff --git a/systems/controllers/c3/c3_controller.cc b/systems/controllers/c3/c3_controller.cc new file mode 100644 index 0000000000..8585005e73 --- /dev/null +++ b/systems/controllers/c3/c3_controller.cc @@ -0,0 +1,279 @@ +#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::C3; +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, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options_); + + } else if (c3_options_.projection_type == "QP") { + c3_ = std::make_unique(lcs_placeholder, + C3::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 new file mode 100644 index 0000000000..e8131583eb --- /dev/null +++ b/systems/controllers/c3/c3_controller.h @@ -0,0 +1,104 @@ +#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.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 new file mode 100644 index 0000000000..fc4cc16a70 --- /dev/null +++ b/systems/controllers/c3/lcs_factory_system.cc @@ -0,0 +1,149 @@ +#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_.num_friction_directions, c3_options_.mu, c3_options_.dt, + c3_options_.N, 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_.num_friction_directions, c3_options_.mu, contact_model); +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/c3/lcs_factory_system.h b/systems/controllers/c3/lcs_factory_system.h new file mode 100644 index 0000000000..a267d308e0 --- /dev/null +++ b/systems/controllers/c3/lcs_factory_system.h @@ -0,0 +1,74 @@ +#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/endeffector_position_controller.cc b/systems/controllers/endeffector_position_controller.cc deleted file mode 100644 index 9b15a51528..0000000000 --- a/systems/controllers/endeffector_position_controller.cc +++ /dev/null @@ -1,111 +0,0 @@ -#include "systems/controllers/endeffector_position_controller.h" -#include - -namespace dairlib{ -namespace systems{ - -EndEffectorPositionController::EndEffectorPositionController( - const MultibodyPlant& plant, std::string ee_frame_name, - Eigen::Vector3d ee_contact_frame, double k_p, double k_omega) - : plant_(plant), plant_world_frame_(plant_.world_frame()), - ee_contact_frame_(ee_contact_frame), - ee_joint_frame_(plant_.GetFrameByName(ee_frame_name)){ - - // Set up this block's input and output ports - // Input port values will be accessed via EvalVectorInput() later - joint_position_measured_port_ = this->DeclareVectorInputPort( - "joint_position_measured", BasicVector(plant_.num_positions())).get_index(); - endpoint_position_commanded_port_ = this->DeclareVectorInputPort( - "endpoint_position_commanded", BasicVector(3)).get_index(); - endpoint_orientation_commanded_port_ = this->DeclareVectorInputPort( - "endpoint_orientation_commanded", BasicVector(4)).get_index(); - endpoint_twist_cmd_output_port_ = - this->DeclareVectorOutputPort( - "endpoint_twist_command", BasicVector(6), - &EndEffectorPositionController::CalcOutputTwist) - .get_index(); - - // The coordinates for the end effector with respect to the last joint. - // Eventually passed into transformPointsJacobian() - k_p_ = k_p; - k_omega_ = k_omega; -} - -void EndEffectorPositionController::CalcOutputTwist( - const Context &context, BasicVector* output) const { - - VectorX q_actual = this->EvalVectorInput(context, - joint_position_measured_port_)->CopyToVector(); - - VectorX x_desired = this->EvalVectorInput(context, - endpoint_position_commanded_port_)->CopyToVector(); - - VectorX orientation_desired = this->EvalVectorInput(context, - endpoint_orientation_commanded_port_)->CopyToVector(); - - - Eigen::Vector3d x_actual; - const std::unique_ptr> plant_context = - plant_.CreateDefaultContext(); - plant_.SetPositions(plant_context.get(), q_actual); - plant_.CalcPointsPositions(*plant_context, ee_joint_frame_, ee_contact_frame_, - plant_world_frame_, &x_actual); - - VectorXd diff = k_p_ * (x_desired - x_actual); - - // Quaternion for rotation from base to end effector - Eigen::Quaternion quat_n_a = plant_.CalcRelativeTransform( - *plant_context, plant_world_frame_, ee_joint_frame_).rotation().ToQuaternion(); - - // Quaternion for rotation from world frame to desired end effector attitude. - Eigen::Quaternion quat_n_a_des = Eigen::Quaternion( - orientation_desired(0), orientation_desired(1), orientation_desired(2), - orientation_desired(3)); - - // Quaternion for rotation - // from end effector attitude to desired end effector attitude. - Eigen::Quaternion quat_a_a_des = - quat_n_a.conjugate().operator*(quat_n_a_des); - - // Angle Axis Representation for the given quaternion - Eigen::AngleAxis angleaxis_a_a_des = - Eigen::AngleAxis(quat_a_a_des); - MatrixXd axis = angleaxis_a_a_des.axis(); - MatrixXd angularVelocity = k_omega_ * axis * angleaxis_a_a_des.angle(); - - // Transforming angular velocity from joint frame to world frame - VectorXd angularVelocityWF = plant_.CalcRelativeTransform( - *plant_context, ee_joint_frame_, plant_world_frame_).rotation() * angularVelocity; - - // TODO: parse these from the json file - // Limit maximum commanded linear velocity - double linear_speed_limit = 3.0; - double currVel = diff.norm(); - - if (currVel > linear_speed_limit || currVel < -linear_speed_limit) { - diff = diff * (linear_speed_limit/currVel); - std::cout << "Warning: desired end effector velocity: " << currVel; - std::cout << " exceeded limit of " << linear_speed_limit << std::endl; - currVel = diff.norm(); - std::cout << "Set end effector velocity to " << currVel << std::endl; - } - - // Limit maximum commanded angular velocity - double angular_speed_limit = 3.0; - double currAngVel = angularVelocityWF.norm(); - if (currAngVel > angular_speed_limit || currAngVel < -angular_speed_limit) { - angularVelocityWF = angularVelocityWF * (angular_speed_limit/currAngVel); - std::cout << "Warning: desired end effector velocity: " << currAngVel; - std::cout << " exceeded limit of " << angular_speed_limit << std::endl; - currAngVel = angularVelocityWF.norm(); - std::cout << "Set end effector angular velocity to " << currAngVel; - std::cout << std::endl; - } - - MatrixXd twist(6, 1); - twist << angularVelocityWF, diff; - output->set_value(twist); -} - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/endeffector_position_controller.h b/systems/controllers/endeffector_position_controller.h deleted file mode 100644 index b17297567b..0000000000 --- a/systems/controllers/endeffector_position_controller.h +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once - -#include "systems/framework/timestamped_vector.h" -#include "drake/systems/framework/leaf_system.h" -#include "drake/systems/framework/basic_vector.h" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/multibody/tree/multibody_tree.h" - -#include -#include -#include - -using Eigen::VectorXd; -using Eigen::MatrixXd; -using Eigen::Quaternion; -using Eigen::Quaterniond; -using Eigen::AngleAxis; -using Eigen::AngleAxisd; -using drake::systems::LeafSystem; -using drake::systems::Context; -using drake::multibody::MultibodyPlant; -using drake::multibody::Frame; - -namespace dairlib{ -namespace systems{ - -// PD Controller for end effector position. -// Takes in desired end effector position and orientation, -// orientation as quaternion, position as 3-vector. -// outputs appropriate velocity (twist) as 6-vector: first three indices -// are desired angular velocity, next three are desired linear velocity. - -class EndEffectorPositionController : public LeafSystem { - public: - // Constructor - EndEffectorPositionController(const MultibodyPlant& plant, - std::string ee_frame_name, - Eigen::Vector3d ee_contact_frame, - double k_p, double k_omega); - - // Getter methods for each of the individual input/output ports. - const drake::systems::InputPort& get_joint_pos_input_port() const { - return this->get_input_port(joint_position_measured_port_); - } - const drake::systems::InputPort& get_endpoint_pos_input_port() const { - return this->get_input_port(endpoint_position_commanded_port_); - } - const drake::systems::InputPort& get_endpoint_orient_input_port() const { - return this->get_input_port(endpoint_orientation_commanded_port_); - } - const drake::systems::OutputPort& get_endpoint_cmd_output_port() const { - return this->get_output_port(endpoint_twist_cmd_output_port_); - } - - private: - // Callback method called when declaring output port of the system. - // Twist combines linear and angular velocities. - void CalcOutputTwist(const Context &context, - BasicVector* output) const; - - const MultibodyPlant& plant_; - const Frame& plant_world_frame_; - Eigen::Vector3d ee_contact_frame_; - const Frame& ee_joint_frame_; - double k_p_; - double k_omega_; - int joint_position_measured_port_; - int endpoint_position_commanded_port_; - int endpoint_orientation_commanded_port_; - int endpoint_twist_cmd_output_port_; - -}; - -} -} diff --git a/systems/controllers/endeffector_velocity_controller.cc b/systems/controllers/endeffector_velocity_controller.cc deleted file mode 100644 index 188c55d867..0000000000 --- a/systems/controllers/endeffector_velocity_controller.cc +++ /dev/null @@ -1,105 +0,0 @@ -#include "systems/controllers/endeffector_velocity_controller.h" -#include - -namespace dairlib{ -namespace systems{ - -EndEffectorVelocityController::EndEffectorVelocityController( - const MultibodyPlant& plant, std::string ee_frame_name, - Eigen::Vector3d ee_contact_frame, double k_d, double k_r) - : plant_(plant), num_joints_(plant_.num_positions()), - ee_joint_frame_(plant_.GetFrameByName(ee_frame_name)){ - - // Set up this block's input and output ports - // Input port values will be accessed via EvalVectorInput() later - joint_position_measured_port_ = this->DeclareVectorInputPort( - "joint_position_measured", BasicVector(num_joints_)).get_index(); - joint_velocity_measured_port_ = this->DeclareVectorInputPort( - "joint_velocity_measured", BasicVector(num_joints_)).get_index(); - endpoint_twist_commanded_port_ = this->DeclareVectorInputPort( - "endpoint_twist_commanded", BasicVector(6)).get_index(); - - // Note that this function contains a pointer to the callback function below. - endpoint_torque_output_port_ = - this->DeclareVectorOutputPort( - "u", BasicVector(num_joints_), - &EndEffectorVelocityController::CalcOutputTorques) - .get_index(); - - ee_contact_frame_ = ee_contact_frame; - k_d_ = k_d; - k_r_ = k_r; -} - -// Callback for DeclareVectorInputPort. No return value. -// The parameter 'output' is the output. -// This function is called many times a second. -void EndEffectorVelocityController::CalcOutputTorques( - const Context& context, BasicVector* output) const { - // We read the above input ports with EvalVectorInput - // The purpose of CopyToVector().head(NUM_JOINTS) is to remove the timestamp - // from the vector input ports - const BasicVector* q_timestamped = - (BasicVector*) this->EvalVectorInput( - context, joint_position_measured_port_); - auto q = q_timestamped->get_value(); - - const BasicVector* q_dot_timestamped = - (BasicVector*) this->EvalVectorInput( - context, joint_velocity_measured_port_); - auto q_dot = q_dot_timestamped->get_value(); - - const BasicVector* twist_desired_timestamped = - (BasicVector*) this->EvalVectorInput( - context, endpoint_twist_commanded_port_); - auto twist_desired = twist_desired_timestamped->get_value(); - - const std::unique_ptr> plant_context = - plant_.CreateDefaultContext(); - plant_.SetPositions(plant_context.get(), q); - plant_.SetVelocities(plant_context.get(), q_dot); - - // Calculating the jacobian of the kuka arm - Eigen::MatrixXd frameSpatialVelocityJacobian(6, num_joints_); - - plant_.CalcJacobianSpatialVelocity(*plant_context, - drake::multibody::JacobianWrtVariable::kV, - ee_joint_frame_, ee_contact_frame_, plant_.world_frame(), - plant_.world_frame(), &frameSpatialVelocityJacobian); - - // Using the jacobian, calculating the actual current velocities of the arm - MatrixXd twist_actual = frameSpatialVelocityJacobian * q_dot; - - // Gains are placed in a diagonal matrix - Eigen::DiagonalMatrix gains(6); - gains.diagonal() << k_r_, k_r_, k_r_, k_d_, k_d_, k_d_; - - // Calculating the error - MatrixXd generalizedForces = gains * (twist_desired - twist_actual); - - // Multiplying J^t x force to get torque outputs - VectorXd commandedTorques(num_joints_); - commandedTorques = frameSpatialVelocityJacobian.transpose() * generalizedForces; - - // Limit maximum commanded torques - double max_torque_limit = 3.0; - for (int i = 0; i < num_joints_; i++) { - if (commandedTorques(i, 0) > max_torque_limit) { - commandedTorques(i, 0) = max_torque_limit; - std::cout << "Warning: joint " << i << " commanded torque exceeded "; - std::cout << "given limit of " << max_torque_limit << std::endl; - } else if (commandedTorques(i, 0) < -max_torque_limit) { - commandedTorques(i, 0) = -max_torque_limit; - std::cout << "Warning: joint " << i << " commanded torque exceeded "; - std::cout << "given limit of " << -max_torque_limit << std::endl; - } - } - - - // Storing them in the output vector - output->set_value(commandedTorques); // (7 x 6) * (6 x 1) = 7 x 1 - -} - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/endeffector_velocity_controller.h b/systems/controllers/endeffector_velocity_controller.h deleted file mode 100644 index 7a68cad445..0000000000 --- a/systems/controllers/endeffector_velocity_controller.h +++ /dev/null @@ -1,69 +0,0 @@ -#pragma once - -#include "common/find_resource.h" -#include "systems/framework/timestamped_vector.h" -#include "drake/systems/framework/leaf_system.h" -#include "drake/systems/framework/basic_vector.h" -#include "drake/multibody/plant/multibody_plant.h" - -#include - -using Eigen::VectorXd; -using Eigen::MatrixXd; -using drake::systems::LeafSystem; -using drake::systems::Context; -using drake::multibody::MultibodyPlant; -using drake::multibody::Frame; - -namespace dairlib{ -namespace systems{ - -// KukaIiwaVelocityController extends LeafSystem, which is basically -// a 'block' like you might see in simulink with inputs and outputs. -// In this case, KukaIiwaVelocityController will take in desired velocities, -// q, q_dot, and output an appropriate torque -// \Tau = jacobian.transpose x K x desired_accelerations -class EndEffectorVelocityController : public LeafSystem { - public: - // Constructor - EndEffectorVelocityController(const MultibodyPlant& plant, - std::string ee_frame_name, - Eigen::Vector3d ee_contact_frame, - double k_d, double k_r); - - // Getter methods for each of the individual input/output ports. - const drake::systems::InputPort& get_joint_pos_input_port() const { - return this->get_input_port(joint_position_measured_port_); - } - const drake::systems::InputPort& get_joint_vel_input_port() const { - return this->get_input_port(joint_velocity_measured_port_); - } - const drake::systems::InputPort& get_endpoint_twist_input_port() const { - return this->get_input_port(endpoint_twist_commanded_port_); - } - const drake::systems::OutputPort& get_endpoint_torque_output_port() const{ - return this->get_output_port(endpoint_torque_output_port_); - } - - private: - // The callback called when declaring the output port of the system. - // The 'output' vector is set in place and then passed out. - // Think a simulink system. - void CalcOutputTorques(const Context& context, - BasicVector* output) const; - - const MultibodyPlant& plant_; - int num_joints_; - const Frame& ee_joint_frame_; - Eigen::Vector3d ee_contact_frame_; - int joint_position_measured_port_; - int joint_velocity_measured_port_; - int endpoint_twist_commanded_port_; - int endpoint_torque_output_port_; - double k_d_; - double k_r_; -}; - - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/gravity_compensator.cc b/systems/controllers/gravity_compensator.cc new file mode 100644 index 0000000000..93fc4370bc --- /dev/null +++ b/systems/controllers/gravity_compensator.cc @@ -0,0 +1,45 @@ +#include "gravity_compensator.h" + +#include "systems/framework/timestamped_vector.h" + +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using drake::systems::LeafSystem; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + + +GravityCompensationRemover::GravityCompensationRemover( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context) + : plant_(plant), context_(context){ + + num_actuators_ = plant_.num_actuators(); + this->DeclareVectorInputPort("u, t", + TimestampedVector(num_actuators_)); + this->DeclareVectorOutputPort("u, t", + TimestampedVector(num_actuators_), + &GravityCompensationRemover::CancelGravityCompensation); +} + +void GravityCompensationRemover::CancelGravityCompensation(const drake::systems::Context& context, + TimestampedVector* output) const { + const TimestampedVector* tau = + (TimestampedVector*)this->EvalVectorInput(context, 0); + VectorXd tau_g = plant_.CalcGravityGeneralizedForces(context_); + + VectorXd compensated_tau = VectorXd::Zero(num_actuators_); + for (int i = 0; i < num_actuators_; i++){ + compensated_tau(i) = tau->GetAtIndex(i) + tau_g(i); + } + + output->SetDataVector(compensated_tau); + output->set_timestamp(tau->get_timestamp()); +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/gravity_compensator.h b/systems/controllers/gravity_compensator.h new file mode 100644 index 0000000000..9d7bcb8cf1 --- /dev/null +++ b/systems/controllers/gravity_compensator.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + + +#include +#include + +#include "systems/framework/output_vector.h" +#include "drake/systems/framework/leaf_system.h" + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/context.h" + +#include +#include +#include + +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using drake::systems::LeafSystem; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + +class GravityCompensationRemover : public LeafSystem { + public: + GravityCompensationRemover( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context); + + private: + // adds gravity compensation to output + void CancelGravityCompensation(const drake::systems::Context& context, + TimestampedVector* output) const; + + // constructor variables + const MultibodyPlant& plant_; + drake::systems::Context& context_; + int num_actuators_; +}; + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/osc/BUILD.bazel b/systems/controllers/osc/BUILD.bazel index 8f7641b61a..f88584fa5a 100644 --- a/systems/controllers/osc/BUILD.bazel +++ b/systems/controllers/osc/BUILD.bazel @@ -42,6 +42,7 @@ cc_library( name = "osc_tracking_datas", deps = [ ":com_tracking_data", + ":external_force_tracking_data", ":joint_space_tracking_data", ":osc_tracking_data", ":relative_transform_tracking_data", @@ -136,6 +137,18 @@ cc_library( ], ) +cc_library( + name = "external_force_tracking_data", + srcs = ["external_force_tracking_data.cc"], + hdrs = ["external_force_tracking_data.h"], + deps = [ + ":osc_tracking_data", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "osc_gains", hdrs = ["osc_gains.h"], diff --git a/systems/controllers/osc/external_force_tracking_data.cc b/systems/controllers/osc/external_force_tracking_data.cc new file mode 100644 index 0000000000..9433b1b7b1 --- /dev/null +++ b/systems/controllers/osc/external_force_tracking_data.cc @@ -0,0 +1,49 @@ +#include "external_force_tracking_data.h" + +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; +using std::string; +using std::vector; + +using drake::multibody::JacobianWrtVariable; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; + +namespace dairlib::systems::controllers { + +ExternalForceTrackingData::ExternalForceTrackingData( + const string& name, const MatrixXd& W, + const MultibodyPlant& plant_w_spr, + const MultibodyPlant& plant_wo_spr, + const std::string& body_name, const Vector3d& pt_on_body) + : name_(name), + plant_w_spr_(plant_w_spr), + plant_wo_spr_(plant_wo_spr), + world_w_spr_(plant_w_spr_.world_frame()), + world_wo_spr_(plant_wo_spr_.world_frame()), + body_frame_w_spr_(&plant_w_spr_.GetBodyByName(body_name).body_frame()), + body_frame_wo_spr_(&plant_wo_spr_.GetBodyByName(body_name).body_frame()), + pt_on_body_(pt_on_body), + W_(W) { + J_ = MatrixXd::Zero(3, plant_wo_spr_.num_velocities()); + lambda_des_ = Vector3d::Zero(); +} + +void ExternalForceTrackingData::Update( + const Eigen::VectorXd& x_w_spr, + const drake::systems::Context& context_w_spr, + const Eigen::VectorXd& x_wo_spr, + const drake::systems::Context& context_wo_spr, + const drake::trajectories::Trajectory& traj, + double t) { + DRAKE_DEMAND(traj.rows() == 3); + lambda_des_ = traj.value(t); + J_ = MatrixXd::Zero(3, plant_wo_spr_.num_velocities()); + plant_wo_spr_.CalcJacobianTranslationalVelocity( + context_wo_spr, JacobianWrtVariable::kV, *body_frame_wo_spr_, pt_on_body_, + world_wo_spr_, world_wo_spr_, &J_); +} + +} // namespace dairlib::systems::controllers diff --git a/systems/controllers/osc/external_force_tracking_data.h b/systems/controllers/osc/external_force_tracking_data.h new file mode 100644 index 0000000000..b97a0aa7d6 --- /dev/null +++ b/systems/controllers/osc/external_force_tracking_data.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include + +namespace dairlib { +namespace systems { +namespace controllers { + +/// ExternalForceTrackingData +/// Force tracking objective. Used to track desired external forces. Requires +/// contact points on the MultibodyPlant where contact forces enter the dynamics +class ExternalForceTrackingData { + public: + ExternalForceTrackingData( + const std::string& name, const Eigen::MatrixXd& W, + const drake::multibody::MultibodyPlant& plant_w_spr, + const drake::multibody::MultibodyPlant& plant_wo_spr, + const std::string& body_name, const Eigen::Vector3d& pt_on_body); + + const Eigen::MatrixXd& GetWeight() const { return W_; } + + const Eigen::MatrixXd& GetJ() const { return J_; } + const Eigen::VectorXd& GetLambdaDes() const { return lambda_des_; } + const std::string& GetName() const { return name_; }; + int GetLambdaDim() const { return n_lambda_; }; + + const drake::multibody::MultibodyPlant& plant_w_spr() const { + return plant_w_spr_; + }; + const drake::multibody::MultibodyPlant& plant_wo_spr() const { + return plant_wo_spr_; + }; + void Update(const Eigen::VectorXd& x_w_spr, + const drake::systems::Context& context_w_spr, + const Eigen::VectorXd& x_wo_spr, + const drake::systems::Context& context_wo_spr, + const drake::trajectories::Trajectory& traj, + double t); + + protected: + private: + std::string name_; + + const drake::multibody::MultibodyPlant& plant_w_spr_; + const drake::multibody::MultibodyPlant& plant_wo_spr_; + // World frames + const drake::multibody::RigidBodyFrame& world_w_spr_; + const drake::multibody::RigidBodyFrame& world_wo_spr_; + + const drake::multibody::RigidBodyFrame* body_frame_w_spr_; + const drake::multibody::RigidBodyFrame* body_frame_wo_spr_; + const Eigen::Vector3d pt_on_body_; + + int n_lambda_ = 3; + + Eigen::VectorXd lambda_des_; + Eigen::MatrixXd J_; + Eigen::MatrixXd W_; +}; + +} // namespace controllers +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/osc/operational_space_control.cc b/systems/controllers/osc/operational_space_control.cc index 9b0db4f84e..748e49a2e0 100644 --- a/systems/controllers/osc/operational_space_control.cc +++ b/systems/controllers/osc/operational_space_control.cc @@ -62,9 +62,9 @@ OperationalSpaceControl::OperationalSpaceControl( n_u_ = plant.num_actuated_dofs(); // Input/Output Setup - state_port_ = - this->DeclareVectorInputPort( - "x, u, t", OutputVector(n_q_,n_v_, n_u_)).get_index(); + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(n_q_, n_v_, n_u_)) + .get_index(); if (used_with_finite_state_machine) { fsm_port_ = @@ -83,10 +83,10 @@ OperationalSpaceControl::OperationalSpaceControl( prev_event_time_idx_ = this->DeclareDiscreteState(VectorXd::Zero(1)); } - osc_output_port_ = this->DeclareVectorOutputPort( - "u, t", TimestampedVector(n_u_), - &OperationalSpaceControl::CalcOptimalInput) - .get_index(); + osc_output_port_ = + this->DeclareVectorOutputPort("u, t", TimestampedVector(n_u_), + &OperationalSpaceControl::CalcOptimalInput) + .get_index(); osc_debug_port_ = this->DeclareAbstractOutputPort( "lcmt_osc_debug", &OperationalSpaceControl::AssignOscLcmOutput) @@ -198,6 +198,27 @@ void OperationalSpaceControl::AddTrackingData( traj_name_to_port_index_map_[traj_name] = port_index; } } + +// Tracking data methods +void OperationalSpaceControl::AddForceTrackingData( + std::unique_ptr tracking_data) { + force_tracking_data_vec_->push_back(std::move(tracking_data)); + + // Construct input ports and add element to traj_name_to_port_index_map_ if + // the port for the traj is not created yet + string traj_name = force_tracking_data_vec_->back()->GetName(); + if (traj_name_to_port_index_map_.find(traj_name) == + traj_name_to_port_index_map_.end()) { + PiecewisePolynomial pp = PiecewisePolynomial(); + int port_index = + this->DeclareAbstractInputPort( + traj_name, + drake::Value>(pp)) + .get_index(); + traj_name_to_port_index_map_[traj_name] = port_index; + } +} + void OperationalSpaceControl::AddConstTrackingData( std::unique_ptr tracking_data, const VectorXd& v, double t_lb, double t_ub) { @@ -253,46 +274,45 @@ void OperationalSpaceControl::Build() { // Add costs // 1. input cost if (W_input_.size() > 0) { - id_qp_.AddQuadraticCost( - "input_cost", W_input_, VectorXd::Zero(n_u_), id_qp_.u()); + id_qp_.AddQuadraticCost("input_cost", W_input_, VectorXd::Zero(n_u_), + id_qp_.u()); } // 2. acceleration cost if (W_joint_accel_.size() > 0) { DRAKE_DEMAND(W_joint_accel_.rows() == n_v_); - id_qp_.AddQuadraticCost( - "acceleration_cost", W_joint_accel_, VectorXd::Zero(n_v_), id_qp_.dv()); + id_qp_.AddQuadraticCost("acceleration_cost", W_joint_accel_, + VectorXd::Zero(n_v_), id_qp_.dv()); } if (W_input_smoothing_.size() > 0) { - id_qp_.AddQuadraticCost( - "input_smoothing_cost", W_input_smoothing_, VectorXd::Zero(n_u_), id_qp_.u()); + id_qp_.AddQuadraticCost("input_smoothing_cost", W_input_smoothing_, + VectorXd::Zero(n_u_), id_qp_.u()); } // 3. contact force cost if (W_lambda_c_reg_.size() > 0) { int nc = id_qp_.lambda_c().rows(); DRAKE_DEMAND(W_lambda_c_reg_.rows() == nc); - id_qp_.AddQuadraticCost( - "lambda_c_cost", W_lambda_c_reg_, VectorXd::Zero(nc), id_qp_.lambda_c()); + id_qp_.AddQuadraticCost("lambda_c_cost", W_lambda_c_reg_, + VectorXd::Zero(nc), id_qp_.lambda_c()); } // 3. constraint force cost if (W_lambda_h_reg_.size() > 0) { int nh = id_qp_.lambda_h().rows(); DRAKE_DEMAND(W_lambda_h_reg_.rows() == nh); - id_qp_.AddQuadraticCost( - "lambda_h_cost", W_lambda_h_reg_, VectorXd::Zero(nh), id_qp_.lambda_h()); + id_qp_.AddQuadraticCost("lambda_h_cost", W_lambda_h_reg_, + VectorXd::Zero(nh), id_qp_.lambda_h()); } // 4. Soft constraint cost if (w_soft_constraint_ > 0) { int nca = id_qp_.nc_active(); - id_qp_.AddQuadraticCost( - "soft_constraint_cost", - w_soft_constraint_ * MatrixXd::Identity(nca, nca), VectorXd::Zero(nca), - id_qp_.epsilon()); + id_qp_.AddQuadraticCost("soft_constraint_cost", + w_soft_constraint_ * MatrixXd::Identity(nca, nca), + VectorXd::Zero(nca), id_qp_.epsilon()); } // 4. Tracking cost for (const auto& data : *tracking_data_vec_) { id_qp_.AddQuadraticCost(data->GetName(), MatrixXd::Zero(n_v_, n_v_), - VectorXd::Zero(n_v_), id_qp_.dv()); + VectorXd::Zero(n_v_), id_qp_.dv()); } // 5. Joint Limit cost @@ -302,7 +322,7 @@ void OperationalSpaceControl::Build() { n_revolute_joints_, n_revolute_joints_); id_qp_.AddQuadraticCost( "joint_limit_cost", - MatrixXd::Zero(n_revolute_joints_,n_revolute_joints_), + MatrixXd::Zero(n_revolute_joints_, n_revolute_joints_), VectorXd::Zero(n_revolute_joints_), id_qp_.dv().tail(n_revolute_joints_)); } @@ -311,12 +331,14 @@ void OperationalSpaceControl::Build() { if (ds_duration_ > 0) { int nc = id_qp_.nc(); const auto& lambda = id_qp_.lambda_c(); - blend_constraint_ = id_qp_.get_mutable_prog().AddLinearEqualityConstraint( - MatrixXd::Zero(1, nc / kSpaceDim), VectorXd::Zero(1), - {lambda.segment(kSpaceDim * 0 + 2, 1), - lambda.segment(kSpaceDim * 1 + 2, 1), - lambda.segment(kSpaceDim * 2 + 2, 1), - lambda.segment(kSpaceDim * 3 + 2, 1)}) + blend_constraint_ = + id_qp_.get_mutable_prog() + .AddLinearEqualityConstraint(MatrixXd::Zero(1, nc / kSpaceDim), + VectorXd::Zero(1), + {lambda.segment(kSpaceDim * 0 + 2, 1), + lambda.segment(kSpaceDim * 1 + 2, 1), + lambda.segment(kSpaceDim * 2 + 2, 1), + lambda.segment(kSpaceDim * 3 + 2, 1)}) .evaluator() .get(); } @@ -355,18 +377,17 @@ VectorXd OperationalSpaceControl::SolveQp( const VectorXd& x_w_spr, const VectorXd& x_wo_spr, const drake::systems::Context& context, double t, int fsm_state, double t_since_last_state_switch, double alpha, int next_fsm_state) const { - // Update context - SetPositionsIfNew( - plant_, x_w_spr.head(plant_.num_positions()), context_); - SetVelocitiesIfNew( - plant_, x_w_spr.tail(plant_.num_velocities()), context_); - - const auto active_contact_names = contact_names_map_.count(fsm_state) > 0 ? - contact_names_map_.at(fsm_state) : std::vector(); + SetPositionsIfNew(plant_, x_w_spr.head(plant_.num_positions()), + context_); + SetVelocitiesIfNew(plant_, x_w_spr.tail(plant_.num_velocities()), + context_); + + const auto active_contact_names = contact_names_map_.count(fsm_state) > 0 + ? contact_names_map_.at(fsm_state) + : std::vector(); id_qp_.UpdateDynamics(x_w_spr, active_contact_names, {}); - // Invariant Impacts // Only update when near an impact bool near_impact = alpha != 0; @@ -376,9 +397,9 @@ VectorXd OperationalSpaceControl::SolveQp( MatrixXd M(n_v_, n_v_); plant_.CalcMassMatrix(*context_, &M); - UpdateImpactInvariantProjection( - x_w_spr, x_wo_spr, context, t, t_since_last_state_switch, - fsm_state, next_fsm_state, M); + UpdateImpactInvariantProjection(x_w_spr, x_wo_spr, context, t, + t_since_last_state_switch, fsm_state, + next_fsm_state, M); // Need to call Update before this to get the updated jacobian v_proj = alpha * M_Jt_ * ii_lambda_sol_ + 1e-13 * VectorXd::Ones(n_v_); } @@ -407,8 +428,7 @@ VectorXd OperationalSpaceControl::SolveQp( const auto& traj = input_traj->get_value>(); // Update - tracking_data->Update(x_w_spr, *context_, x_wo_spr, - *context_, traj, t, + tracking_data->Update(x_w_spr, *context_, x_wo_spr, *context_, traj, t, t_since_last_state_switch, fsm_state, v_proj); } @@ -418,28 +438,29 @@ VectorXd OperationalSpaceControl::SolveQp( const VectorXd& JdotV_t = tracking_data->GetJdotTimesV(); const VectorXd constant_term = (JdotV_t - ddy_t); - id_qp_.UpdateCost( - tracking_data->GetName(), - 2 * J_t.transpose() * W * J_t, - 2 * J_t.transpose() * W * (JdotV_t - ddy_t), - constant_term.transpose() * W * constant_term); + id_qp_.UpdateCost(tracking_data->GetName(), 2 * J_t.transpose() * W * J_t, + 2 * J_t.transpose() * W * (JdotV_t - ddy_t), + constant_term.transpose() * W * constant_term); } else { - id_qp_.UpdateCost( - tracking_data->GetName(), MatrixXd::Zero(n_v_, n_v_), - VectorXd::Zero(n_v_)); + id_qp_.UpdateCost(tracking_data->GetName(), MatrixXd::Zero(n_v_, n_v_), + VectorXd::Zero(n_v_)); } } // Add joint limit constraints if (w_joint_limit_ > 0) { VectorXd w_joint_limit = - K_joint_pos_ * (x_wo_spr.head(plant_.num_positions()) - .tail(n_revolute_joints_) -q_max_).cwiseMax(0) + - K_joint_pos_ * (x_wo_spr.head(plant_.num_positions()) - .tail(n_revolute_joints_) - q_min_).cwiseMin(0); - id_qp_.UpdateCost( - "joint_limit_cost", - MatrixXd::Zero(n_revolute_joints_, n_revolute_joints_), w_joint_limit); + K_joint_pos_ * + (x_wo_spr.head(plant_.num_positions()).tail(n_revolute_joints_) - + q_max_) + .cwiseMax(0) + + K_joint_pos_ * + (x_wo_spr.head(plant_.num_positions()).tail(n_revolute_joints_) - + q_min_) + .cwiseMin(0); + id_qp_.UpdateCost("joint_limit_cost", + MatrixXd::Zero(n_revolute_joints_, n_revolute_joints_), + w_joint_limit); } // TODO (@Brian-Acosta) test double support blending as a force cost @@ -482,20 +503,19 @@ VectorXd OperationalSpaceControl::SolveQp( // (Testing) 7. Cost for staying close to the previous input if (W_input_smoothing_.size() > 0 && u_prev_) { id_qp_.UpdateCost( - "input_smoothing_cost", W_input_smoothing_, -W_input_smoothing_ * *u_prev_, + "input_smoothing_cost", W_input_smoothing_, + -W_input_smoothing_ * *u_prev_, 0.5 * u_prev_->transpose() * W_input_smoothing_ * *u_prev_); } if (W_lambda_c_reg_.size() > 0) { - id_qp_.UpdateCost( - "lambda_c_cost", - (1 + alpha) * W_lambda_c_reg_,VectorXd::Zero(id_qp_.nc())); + id_qp_.UpdateCost("lambda_c_cost", (1 + alpha) * W_lambda_c_reg_, + VectorXd::Zero(id_qp_.nc())); } if (W_lambda_h_reg_.size() > 0) { - id_qp_.UpdateCost( - "lambda_h_reg", - (1 + alpha) * W_lambda_h_reg_,VectorXd::Zero(id_qp_.nh())); + id_qp_.UpdateCost("lambda_h_reg", (1 + alpha) * W_lambda_h_reg_, + VectorXd::Zero(id_qp_.nh())); } if (!solver_->IsInitialized()) { solver_->InitializeSolver(id_qp_.get_prog(), solver_options_); @@ -533,7 +553,6 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( const VectorXd& x_w_spr, const VectorXd& x_wo_spr, const Context& context, double t, double t_since_last_state_switch, int fsm_state, int next_fsm_state, const MatrixXd& M) const { - auto map_iterator = contact_names_map_.find(next_fsm_state); if (map_iterator == contact_names_map_.end()) { @@ -582,8 +601,7 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( EvalAbstractInput(context, port_index); const auto& traj = input_traj->get_value>(); - tracking_data->Update(x_w_spr, *context_, x_wo_spr, - *context_, traj, t, + tracking_data->Update(x_w_spr, *context_, x_wo_spr, *context_, traj, t, t_since_last_state_switch, fsm_state, v_proj); } } @@ -611,14 +629,14 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( VectorXd b_constrained = VectorXd::Zero(active_constraint_dim + id_qp_.nh()); VectorXd Ab = A.transpose() * ydot_err_vec; if (id_qp_.nh() > 0) { - MatrixXd J_h = id_qp_.get_holonomic_evaluators().EvalFullJacobian( - *context_); + MatrixXd J_h = + id_qp_.get_holonomic_evaluators().EvalFullJacobian(*context_); MatrixXd C = J_h * M_Jt_; VectorXd d = J_h * x_w_spr.tail(n_v_); - A_constrained.block(active_constraint_dim, 0, id_qp_.nh(), active_constraint_dim) = - C; - A_constrained.block(0, active_constraint_dim, active_constraint_dim, id_qp_.nh()) = - C.transpose(); + A_constrained.block(active_constraint_dim, 0, id_qp_.nh(), + active_constraint_dim) = C; + A_constrained.block(0, active_constraint_dim, active_constraint_dim, + id_qp_.nh()) = C.transpose(); b_constrained << Ab, d; } else { b_constrained << Ab; @@ -645,14 +663,13 @@ void OperationalSpaceControl::AssignOscLcmOutput( output->fsm_state = fsm_state; const std::vector> - potential_regularization_cost_names_and_vars { - {"input_cost", *u_sol_}, - {"acceleration_cost", *dv_sol_}, - {"soft_constraint_cost", *epsilon_sol_}, - {"input_smoothing_cost", *u_sol_}, - {"lambda_c_cost", *lambda_c_sol_}, - {"lambda_h_cost", *lambda_h_sol_} - }; + potential_regularization_cost_names_and_vars{ + {"input_cost", *u_sol_}, + {"acceleration_cost", *dv_sol_}, + {"soft_constraint_cost", *epsilon_sol_}, + {"input_smoothing_cost", *u_sol_}, + {"lambda_c_cost", *lambda_c_sol_}, + {"lambda_h_cost", *lambda_h_sol_}}; output->regularization_cost_names.clear(); output->regularization_costs.clear(); @@ -724,9 +741,8 @@ void OperationalSpaceControl::AssignOscLcmOutput( CopyVectorXdToStdVector(tracking_data->GetYddotCommandSol()); VectorXd y_tracking_cost = VectorXd::Zero(1); - id_qp_.get_cost_evaluator( - tracking_data->GetName() - ).Eval(*dv_sol_, &y_tracking_cost); + id_qp_.get_cost_evaluator(tracking_data->GetName()) + .Eval(*dv_sol_, &y_tracking_cost); total_cost += y_tracking_cost[0]; output->tracking_costs.push_back(y_tracking_cost[0]); output->tracking_data.push_back(osc_output); @@ -796,8 +812,8 @@ void OperationalSpaceControl::CheckTracking( output->get_mutable_value()(0) = 0.0; VectorXd y_soft_constraint_cost = VectorXd::Zero(1); if (id_qp_.has_cost_named("soft_constraint_cost")) { - id_qp_.get_cost_evaluator("soft_constraint_cost").Eval( - *epsilon_sol_, &y_soft_constraint_cost); + id_qp_.get_cost_evaluator("soft_constraint_cost") + .Eval(*epsilon_sol_, &y_soft_constraint_cost); } if (y_soft_constraint_cost[0] > 1e5 || isnan(y_soft_constraint_cost[0])) { output->get_mutable_value()(0) = 1.0; diff --git a/systems/controllers/osc/operational_space_control.h b/systems/controllers/osc/operational_space_control.h index ca787afb65..7271ca58ed 100644 --- a/systems/controllers/osc/operational_space_control.h +++ b/systems/controllers/osc/operational_space_control.h @@ -17,10 +17,11 @@ #include "solvers/fast_osqp_solver.h" #include "solvers/solver_options_io.h" #include "systems/controllers/control_utils.h" +#include "systems/controllers/osc/external_force_tracking_data.h" +#include "systems/controllers/osc/inverse_dynamics_qp.h" #include "systems/controllers/osc/osc_tracking_data.h" #include "systems/framework/impact_info_vector.h" #include "systems/framework/output_vector.h" -#include "systems/controllers/osc/inverse_dynamics_qp.h" #include "drake/common/trajectories/exponential_plus_piecewise_polynomial.h" #include "drake/common/trajectories/piecewise_polynomial.h" @@ -97,10 +98,9 @@ namespace dairlib::systems::controllers { class OperationalSpaceControl : public drake::systems::LeafSystem { public: - OperationalSpaceControl( - const drake::multibody::MultibodyPlant& plant, - drake::systems::Context* context, - bool used_with_finite_state_machine = true); + OperationalSpaceControl(const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + bool used_with_finite_state_machine = true); /***** Input/output ports *****/ @@ -221,17 +221,29 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { */ void SetJointLimitWeight(const double w) { w_joint_limit_ = w; } + /*! + * @brief Enables limits on the joint acceleration variables + */ + void SetAccelerationConstraints(bool constraint_status) { + if (ddq_max_.isZero() and constraint_status) { + throw std::runtime_error( + "Attempting to set acceleration limits when acceleration limits have " + "not been defined for the plant."); + } + with_acceleration_constraints_ = constraint_status; + } + // Constraint methods void SetContactFriction(double mu) { mu_ = mu; } void AddContactPoint( const std::string& name, std::unique_ptr> evaluator, - std::vector fsm_states={}); + std::vector fsm_states = {}); void AddKinematicConstraint( std::unique_ptr> - evaluator); + evaluator); // Tracking data methods /// The third argument is used to set a period in which OSC does not track the @@ -240,6 +252,8 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { void AddTrackingData(std::unique_ptr tracking_data, double t_lb = 0, double t_ub = std::numeric_limits::infinity()); + void AddForceTrackingData( + std::unique_ptr tracking_data); void AddConstTrackingData( std::unique_ptr tracking_data, const Eigen::VectorXd& v, double t_lb = 0, double t_ub = std::numeric_limits::infinity()); @@ -262,8 +276,7 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { SetOsqpSolverOptions( drake::yaml::LoadYamlFile( FindResourceOrThrow(yaml_string)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()) - ); + .GetAsSolverOptions(drake::solvers::OsqpSolver::id())); }; // OSC LeafSystem builder void Build(); @@ -356,6 +369,10 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { Eigen::MatrixXd K_joint_pos_; Eigen::MatrixXd K_joint_vel_; + // robot joint acceleration limits + Eigen::VectorXd ddq_min_; + Eigen::VectorXd ddq_max_; + // flag indicating whether using osc with finite state machine or not bool used_with_finite_state_machine_; @@ -395,6 +412,9 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { std::map> fsm_to_w_input_map_; // each pair is (joint index, weight) + // flags to enable constraints on decision variables + bool with_acceleration_constraints_ = false; + // Soft contact penalty coefficient and friction cone coefficient double mu_ = -1; // Friction coefficients double w_soft_constraint_ = -1; @@ -406,6 +426,9 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { std::unique_ptr>> tracking_data_vec_ = std::make_unique>>(); + std::unique_ptr>> + force_tracking_data_vec_ = std::make_unique< + std::vector>>(); // Fixed position of constant trajectories std::vector fixed_position_vec_; diff --git a/systems/controllers/osc/options_tracking_data.cc b/systems/controllers/osc/options_tracking_data.cc index 9f9023f5ab..1d01660ea5 100644 --- a/systems/controllers/osc/options_tracking_data.cc +++ b/systems/controllers/osc/options_tracking_data.cc @@ -195,7 +195,7 @@ void OptionsTrackingData::SetTimerVaryingFeedForwardAccelMultiplier( ff_accel_multiplier_traj_ = ff_accel_multiplier_traj; } -void OptionsTrackingData::SetCmdAccelerationBounds(Eigen::VectorXd& lb, Eigen::VectorXd& ub){ +void OptionsTrackingData::SetCmdAccelerationBounds(const Eigen::VectorXd& lb, const Eigen::VectorXd& ub){ DRAKE_DEMAND(lb.size() == n_ydot_); DRAKE_DEMAND(ub.size() == n_ydot_); yddot_cmd_lb_ = lb; diff --git a/systems/controllers/osc/options_tracking_data.h b/systems/controllers/osc/options_tracking_data.h index eff035991c..21485e226d 100644 --- a/systems/controllers/osc/options_tracking_data.h +++ b/systems/controllers/osc/options_tracking_data.h @@ -42,7 +42,7 @@ class OptionsTrackingData : public OscTrackingData { std::shared_ptr> ff_accel_multiplier_traj); - void SetCmdAccelerationBounds(Eigen::VectorXd& lb, Eigen::VectorXd& ub); + void SetCmdAccelerationBounds(const Eigen::VectorXd& lb, const Eigen::VectorXd& ub); void SetViewFrame(std::shared_ptr> view_frame) { view_frame_ = view_frame; diff --git a/systems/controllers/osc/osc_gains.h b/systems/controllers/osc/osc_gains.h index 059b05e05a..c853f20a73 100644 --- a/systems/controllers/osc/osc_gains.h +++ b/systems/controllers/osc/osc_gains.h @@ -23,6 +23,7 @@ struct OSCGains { MatrixXd W_acceleration; MatrixXd W_input_regularization; + MatrixXd W_input_smoothing_regularization; MatrixXd W_lambda_c_regularization; MatrixXd W_lambda_h_regularization; @@ -56,6 +57,7 @@ struct OSCGains { this->W_lambda_h_reg.data(), this->W_lambda_h_reg.size()); W_acceleration = w_acceleration.asDiagonal(); W_input_regularization = w_input_regularization.asDiagonal(); + W_input_smoothing_regularization = w_input_reg * w_input_regularization.asDiagonal(); W_lambda_c_regularization = w_lambda_c_regularization.asDiagonal(); W_lambda_h_regularization = w_lambda_h_regularization.asDiagonal(); } diff --git a/systems/controllers/osc/rot_space_tracking_data.cc b/systems/controllers/osc/rot_space_tracking_data.cc index 60e0174287..efb353ea72 100644 --- a/systems/controllers/osc/rot_space_tracking_data.cc +++ b/systems/controllers/osc/rot_space_tracking_data.cc @@ -80,22 +80,12 @@ void RotTaskSpaceTrackingData::UpdateYdot( } void RotTaskSpaceTrackingData::UpdateYdotError(const Eigen::VectorXd& v_proj) { - // Transform qdot to w - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond dy_quat_des(ydot_des_(0), ydot_des_(1), ydot_des_(2), - ydot_des_(3)); - Vector3d w_des_ = 2 * (dy_quat_des * y_quat_des.conjugate()).vec(); - // Because we transform the error here rather than in the parent - // options_tracking_data, and because J_y is already transformed in the view - // frame, we need to undo the transformation on J_y + DRAKE_DEMAND(ydot_des_.size() == 3); error_ydot_ = - w_des_ - ydot_ - view_frame_rot_T_.transpose() * GetJ() * v_proj; + ydot_des_ - ydot_; if (with_view_frame_) { error_ydot_ = view_frame_rot_T_ * error_ydot_; } - - ydot_des_ = - w_des_; // Overwrite 4d quat_dot with 3d omega. Need this for osc logging } void RotTaskSpaceTrackingData::UpdateJ(const VectorXd& x_wo_spr, @@ -120,12 +110,8 @@ void RotTaskSpaceTrackingData::UpdateJdotV( } void RotTaskSpaceTrackingData::UpdateYddotDes(double, double) { - // Convert ddq into angular acceleration - // See https://physics.stackexchange.com/q/460311 - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond yddot_quat_des(yddot_des_(0), yddot_des_(1), yddot_des_(2), - yddot_des_(3)); - yddot_des_converted_ = 2 * (yddot_quat_des * y_quat_des.conjugate()).vec(); + DRAKE_DEMAND(yddot_des_.size() == 3); + yddot_des_converted_ = yddot_des_; if (!idx_zero_feedforward_accel_.empty()) { std::cerr << "RotTaskSpaceTrackingData does not support zero feedforward " "acceleration"; diff --git a/systems/controllers/safe_velocity_controller.cc b/systems/controllers/safe_velocity_controller.cc deleted file mode 100644 index 3c4ab58b46..0000000000 --- a/systems/controllers/safe_velocity_controller.cc +++ /dev/null @@ -1,67 +0,0 @@ -#include "systems/controllers/safe_velocity_controller.h" - -namespace dairlib{ -namespace systems{ - - // Velocity and torque passthrough system that kills the velocity and - // torque commands if it goes above max_velocity. - // Remember to use std::move on the rigid body tree argument. -SafeVelocityController::SafeVelocityController( - double max_velocity, int num_joints) { - - // Set up this block's input and output ports - // Input port values will be accessed via EvalVectorInput() later - joint_torques_input_port_ = this->DeclareVectorInputPort( - "joint_torques_input", BasicVector(num_joints)).get_index(); - joint_velocities_input_port_ = this->DeclareVectorInputPort( - "joint_velocites_input", BasicVector(num_joints)).get_index(); - joint_torques_output_port_ = - this->DeclareVectorOutputPort("u", BasicVector(7), - &SafeVelocityController::CalcOutputTorques) - .get_index(); - - this->DeclareDiscreteState(1); - this->DeclarePerStepDiscreteUpdateEvent( - &SafeVelocityController::CheckTerminate); - - max_velocity_ = max_velocity; -} - -void SafeVelocityController::CalcOutputTorques( - const Context &context, BasicVector* output) const { - - VectorX torques_in = this->EvalVectorInput(context, - joint_torques_input_port_)->CopyToVector(); - - VectorX velocities_in = this->EvalVectorInput(context, - joint_velocities_input_port_)->CopyToVector(); - - double terminated = context.get_discrete_state()[0]; - - if (terminated > 0.5) { - Eigen::VectorXd zeros = Eigen::VectorXd::Zero(7); - torques_in = zeros; - } - - output->set_value(torques_in); -} - -drake::systems::EventStatus SafeVelocityController::CheckTerminate( - const Context &context, - drake::systems::DiscreteValues* next_state) const { - - VectorX velocities_in = this->EvalVectorInput(context, - joint_velocities_input_port_)->CopyToVector(); - - if (velocities_in.maxCoeff() > max_velocity_ || - velocities_in.minCoeff() < -max_velocity_) { - (*next_state)[0] = 1.0; - drake::log()->error("Velocity out of bounds, killing"); - } - - return drake::systems::EventStatus::Succeeded(); -} - - -} // namespace systems -} // namespace dairlib diff --git a/systems/controllers/safe_velocity_controller.h b/systems/controllers/safe_velocity_controller.h deleted file mode 100644 index 28d4d18853..0000000000 --- a/systems/controllers/safe_velocity_controller.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include "common/find_resource.h" -#include "systems/framework/timestamped_vector.h" -#include "drake/systems/framework/leaf_system.h" -#include "drake/systems/framework/basic_vector.h" -#include "drake/systems/framework/event_status.h" -#include "drake/systems/framework/discrete_values.h" - -using Eigen::VectorXd; -using drake::systems::LeafSystem; -using drake::systems::Context; - -namespace dairlib{ -namespace systems{ - -class SafeVelocityController : public LeafSystem { - public: - SafeVelocityController(double max_velocity, int num_joints); - - const drake::systems::InputPort& get_joint_torques_input_port() const { - return this->get_input_port(joint_torques_input_port_); - } - const drake::systems::InputPort& get_joint_velocities_input_port() const { - return this->get_input_port(joint_velocities_input_port_); - } - const drake::systems::OutputPort& get_joint_torques_output_port() const { - return this->get_output_port(joint_torques_output_port_); - } - - private: - void CalcOutputTorques(const Context &context, - BasicVector* output) const; - - drake::systems::EventStatus CheckTerminate( - const Context &context, - drake::systems::DiscreteValues* next_state) const; - - - int joint_torques_input_port_; - int joint_velocities_input_port_; - int joint_torques_output_port_; - - double max_velocity_; -}; -} -} diff --git a/systems/framework/BUILD.bazel b/systems/framework/BUILD.bazel index a762800015..2b9b27766a 100644 --- a/systems/framework/BUILD.bazel +++ b/systems/framework/BUILD.bazel @@ -9,11 +9,13 @@ cc_library( srcs = [ "impact_info_vector.cc", "output_vector.cc", + "state_vector.cc", "timestamped_vector.cc", ], hdrs = [ "impact_info_vector.h", "output_vector.h", + "state_vector.h", "timestamped_vector.h", ], deps = [ diff --git a/systems/framework/geared_motor.cc b/systems/framework/geared_motor.cc index 4ec0d7b731..ddba8e082e 100644 --- a/systems/framework/geared_motor.cc +++ b/systems/framework/geared_motor.cc @@ -8,6 +8,7 @@ namespace systems { using drake::multibody::JointActuator; using drake::multibody::MultibodyPlant; using drake::systems::kUseDefaultName; +using drake::systems::BasicVector; GearedMotor::GearedMotor(const MultibodyPlant& plant, const std::unordered_map& max_motor_speeds) @@ -61,4 +62,4 @@ void GearedMotor::CalcTorqueOutput( } } // namespace systems -} // namespace dairlib +} // namespace dairlib \ No newline at end of file diff --git a/systems/framework/geared_motor.h b/systems/framework/geared_motor.h index 5f765482c3..dc03255a7b 100644 --- a/systems/framework/geared_motor.h +++ b/systems/framework/geared_motor.h @@ -39,7 +39,7 @@ class GearedMotor final : public drake::systems::LeafSystem { protected: void CalcTorqueOutput(const drake::systems::Context& context, - systems::BasicVector* output) const; + drake::systems::BasicVector* output) const; private: bool is_abstract() const { return false; } diff --git a/systems/framework/state_vector.cc b/systems/framework/state_vector.cc new file mode 100644 index 0000000000..60ae8e4de5 --- /dev/null +++ b/systems/framework/state_vector.cc @@ -0,0 +1,6 @@ +#include "systems/framework/state_vector.h" + +#include "drake/common/default_scalars.h" + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::dairlib::systems::StateVector) diff --git a/systems/framework/state_vector.h b/systems/framework/state_vector.h new file mode 100644 index 0000000000..506933dd12 --- /dev/null +++ b/systems/framework/state_vector.h @@ -0,0 +1,131 @@ +#pragma once + +#include "systems/framework/timestamped_vector.h" +#include +#include + + +namespace dairlib { +namespace systems { + +using drake::VectorX; +using std::string; +using std::vector; + +/// StateVector stores the object state as a TimestampedVector +/// * positions +/// * velocities +/// Similar to OutputVector but only the state variables +template +class StateVector : public TimestampedVector { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(StateVector) + + StateVector() = default; + + /// Initializes with the given @p size using the drake::dummy_value, which + /// is NaN when T = double. + explicit StateVector(int num_positions, int num_velocities) + : TimestampedVector(num_positions + num_velocities), + num_positions_(num_positions), + num_velocities_(num_velocities), + position_start_(0){} + + /// Constructs a StateVector with the specified positions and velocities. + explicit StateVector(const VectorX& positions, + const VectorX& velocities) + : StateVector(positions.size(), velocities.size()) { + this->SetPositions(positions); + this->SetVelocities(velocities); + } + + void SetPositions(VectorX positions) { + this->get_mutable_data().segment(position_start_, + num_positions_) = positions; + } + + void SetVelocities(VectorX velocities) { + this->get_mutable_data().segment(position_start_ + num_positions_, + num_velocities_) = velocities; + } + + void SetPositionAtIndex(int index, T value) { + this->SetAtIndex(position_start_ + index, value); + } + + void SetVelocityAtIndex(int index, T value) { + this->SetAtIndex(position_start_ + num_positions_ + index, value); + } + + void SetState(VectorX state) { + this->get_mutable_data().segment(position_start_, + num_positions_ + num_velocities_) = state; + } + + /// Returns a const state vector + const VectorX GetState() const { + return this->get_data().segment(position_start_, + num_positions_ + num_velocities_); + } + + /// Returns a const positions vector + const VectorX GetPositions() const { + return this->get_data().segment(position_start_, num_positions_); + } + + /// Returns a const velocities vector + const VectorX GetVelocities() const { + return this->get_data().segment(position_start_ + num_positions_, + num_velocities_); + } + + /// Returns a mutable state vector + Eigen::Map> GetMutableState() { + auto data = this->get_mutable_data().segment(position_start_, + num_positions_ + num_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable positions vector + Eigen::Map> GetMutablePositions() { + auto data = this->get_mutable_data().segment(position_start_, num_positions_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable velocities vector + Eigen::Map> GetMutableVelocities() { + auto data = this->get_mutable_data().segment( + position_start_ + num_positions_, num_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + T GetPositionAtIndex(int index) const { + return this->GetAtIndex(position_start_ + index); + } + + T GetVelocityAtIndex(int index) const { + return this->GetAtIndex(position_start_ + num_positions_ + index); + } + + void SetName(int index, string name) { + position_names_[index] = name; + } + + string GetName(int index) { + return position_names_[index]; + } + + protected: + virtual StateVector* DoClone() const { + return new StateVector(num_positions_, num_velocities_); + } + + private: + const int num_positions_; + const int num_velocities_; + const int position_start_; + vector position_names_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/framework/timestamped_vector.h b/systems/framework/timestamped_vector.h index ef93cec31b..471a9dde22 100644 --- a/systems/framework/timestamped_vector.h +++ b/systems/framework/timestamped_vector.h @@ -5,31 +5,29 @@ namespace dairlib { namespace systems { -using drake::systems::BasicVector; -using drake::VectorX; - -/// TimestampedVector wraps a BasicVector along with a timestamp field -/// The primary purpose of this is to pass-through a message (e.g. LCM) timestamp -/// Uses a length N+1 BasicVector to store a vector of length N and a timestamp -/// The timestamp is stored as the final element (Nth) +/// TimestampedVector wraps a drake::systems::BasicVector along with a timestamp field +/// The primary purpose of this is to pass-through a message (e.g. LCM) +/// timestamp Uses a length N+1 drake::systems::BasicVector to store a vector of length N and a +/// timestamp The timestamp is stored as the final element (Nth) template -class TimestampedVector : public drake::systems::BasicVector { -public: +class TimestampedVector : public drake::systems::BasicVector { + public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TimestampedVector) /// Constructs an empty TimestampedVector. TimestampedVector() = default; - /// Initializes with the given @p size using the drake::dummy_value, which /// is NaN when T = double. explicit TimestampedVector(int data_size) - : BasicVector(data_size + 1), timestep_index_(data_size) {} + : drake::systems::BasicVector(data_size + 1), + timestep_index_(data_size), + data_size_(data_size) {} /// Constructs a TimestampedVector with the specified @p data. - explicit TimestampedVector(const VectorX& data) + explicit TimestampedVector(const drake::VectorX& data) : TimestampedVector(data.size()) { - VectorX data_timestamped = VectorX(data.size() + 1); + drake::VectorX data_timestamped = drake::VectorX(data.size() + 1); data_timestamped.head(data.size()) = data; data_timestamped(data.size()) = 0; this->SetFromVector(data_timestamped); @@ -48,10 +46,10 @@ class TimestampedVector : public drake::systems::BasicVector { this->SetAtIndex(timestep_index_, timestamp); } - T get_timestamp() const {return this->GetAtIndex(timestep_index_);} + T get_timestamp() const { return this->GetAtIndex(timestep_index_); } - /// Copies the entire vector to a new TimestampedVector, with the same concrete - /// implementation type. + /// Copies the entire vector to a new TimestampedVector, with the same + /// concrete implementation type. /// /// Uses the Non-Virtual Interface idiom because smart pointers do not have /// type covariance. @@ -62,42 +60,44 @@ class TimestampedVector : public drake::systems::BasicVector { } /// Returns the vector without the timestamp - VectorX CopyVectorNoTimestamp() const { + drake::VectorX CopyVectorNoTimestamp() const { return this->CopyToVector().head(timestep_index_); } /// Returns a mutable vector of the data values (without timestamp) - Eigen::Map> get_mutable_data() { + Eigen::Map> get_mutable_data() { auto data = this->get_mutable_value().head(timestep_index_); - return Eigen::Map>(&data(0), data.size()); + return Eigen::Map>(&data(0), data.size()); } /// Returns the entire vector as a const Eigen::VectorBlock. - const VectorX get_data() const { + const drake::VectorX get_data() const { return this->get_value().head(timestep_index_); } - - //sets the data part of the vector (without timestamp) - void SetDataVector(const Eigen::Ref>& value) { + // sets the data part of the vector (without timestamp) + void SetDataVector(const Eigen::Ref>& value) { this->get_mutable_data() = value; } + int data_size() const { return data_size_; } + protected: /// Returns a new TimestampedVector containing a copy of the entire vector. /// Caller must take ownership, and may rely on the NVI wrapper to initialize /// the clone elementwise. /// - /// Subclasses of TimestampedVector must override DoClone to return their covariant - /// type. + /// Subclasses of TimestampedVector must override DoClone to return their + /// covariant type. /// virtual TimestampedVector* DoClone() const { return new TimestampedVector(timestep_index_); } - private: - const int timestep_index_; - }; + private: + const int timestep_index_; + const int data_size_; +}; } // namespace systems } // namespace dairlib diff --git a/systems/primitives/BUILD.bazel b/systems/primitives/BUILD.bazel index e54463d314..6a1eb747b7 100644 --- a/systems/primitives/BUILD.bazel +++ b/systems/primitives/BUILD.bazel @@ -37,6 +37,21 @@ cc_library( ], ) +cc_library( + name = "radio_parser", + srcs = [ + "radio_parser.cc", + ], + hdrs = [ + "radio_parser.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "gaussian_noise_pass_through", srcs = ["gaussian_noise_pass_through.cc"], diff --git a/systems/primitives/radio_parser.cc b/systems/primitives/radio_parser.cc new file mode 100644 index 0000000000..9f26acc05e --- /dev/null +++ b/systems/primitives/radio_parser.cc @@ -0,0 +1,49 @@ +#include "systems/primitives/radio_parser.h" + +namespace dairlib { +namespace systems { + +using drake::systems::BasicVector; +RadioParser::RadioParser() { + data_input_port_ = + this->DeclareVectorInputPort("raw_radio", BasicVector(2 + 16)) + .get_index(); + this->DeclareAbstractOutputPort("radio_out", dairlib::lcmt_radio_out(), + &RadioParser::CalcRadioOutput); +} + +void RadioParser::CalcRadioOutput( + const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const { + const BasicVector& data = + *this->template EvalVectorInput(context, data_input_port_); + output->radioReceiverSignalGood = data[0]; + output->receiverMedullaSignalGood = data[1]; + for (int i = 0; i < 16; ++i) { + output->channel[i] = data[2 + i]; + } +} + +RadioToVector::RadioToVector() { + radio_port_ = this->DeclareAbstractInputPort( + "radio_in", drake::Value()) + .get_index(); + this->DeclareVectorOutputPort("raw_radio", BasicVector(2 + 16), + &RadioToVector::ConvertToVector); +} + +void RadioToVector::ConvertToVector( + const drake::systems::Context& context, + drake::systems::BasicVector* output) const { + const auto& radio_out = + this->EvalInputValue(context, radio_port_); + output->SetZero(); + for (int i = 0; i < 16; ++i) { + output->get_mutable_value()[i] = radio_out->channel[i]; + } + output->get_mutable_value()[16 + 0] = radio_out->radioReceiverSignalGood; + output->get_mutable_value()[16 + 1] = radio_out->receiverMedullaSignalGood; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/primitives/radio_parser.h b/systems/primitives/radio_parser.h new file mode 100644 index 0000000000..f6886d00aa --- /dev/null +++ b/systems/primitives/radio_parser.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +class RadioParser : public drake::systems::LeafSystem { + public: + RadioParser(); + + const drake::systems::InputPort& get_input_port() const { + return drake::systems::LeafSystem::get_input_port(0); + } + + const drake::systems::OutputPort& get_output_port() const { + return drake::systems::LeafSystem::get_output_port(0); + } + + protected: + void CalcRadioOutput(const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const; + + private: + bool is_abstract() const { return false; } + int data_input_port_; +}; + +class RadioToVector : public drake::systems::LeafSystem { + public: + RadioToVector(); + + const drake::systems::InputPort& get_input_port() const { + return drake::systems::LeafSystem::get_input_port(0); + } + + const drake::systems::OutputPort& get_output_port() const { + return drake::systems::LeafSystem::get_output_port(0); + } + + protected: + void ConvertToVector(const drake::systems::Context& context, + drake::systems::BasicVector* output) const; + + private: + bool is_abstract() const { return false; } + int radio_port_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/robot_lcm_systems.cc b/systems/robot_lcm_systems.cc index b812225a48..b9371c0072 100644 --- a/systems/robot_lcm_systems.cc +++ b/systems/robot_lcm_systems.cc @@ -1,7 +1,7 @@ -#include - #include "robot_lcm_systems.h" +#include "dairlib/lcmt_robot_input.hpp" +#include "dairlib/lcmt_robot_output.hpp" #include "multibody/multibody_utils.h" #include "drake/multibody/plant/multibody_plant.h" @@ -9,19 +9,16 @@ #include "drake/systems/lcm/lcm_subscriber_system.h" #include "drake/systems/primitives/discrete_time_delay.h" -#include "dairlib/lcmt_robot_input.hpp" -#include "dairlib/lcmt_robot_output.hpp" - namespace dairlib { namespace systems { using drake::multibody::JointActuatorIndex; using drake::multibody::JointIndex; using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; using drake::systems::Context; using drake::systems::lcm::LcmPublisherSystem; using drake::systems::lcm::LcmSubscriberSystem; -using drake::systems::LeafSystem; using Eigen::VectorXd; using std::string; using systems::OutputVector; @@ -36,6 +33,10 @@ RobotOutputReceiver::RobotOutputReceiver( num_efforts_ = plant.num_actuators(); position_index_map_ = multibody::MakeNameToPositionsMap(plant); velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + model_instance_ = drake::multibody::ModelInstanceIndex(-1); + + positions_start_idx_ = 0; + velocities_start_idx_ = 0; effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_output", drake::Value{}); @@ -46,6 +47,35 @@ RobotOutputReceiver::RobotOutputReceiver( &RobotOutputReceiver::CopyOutput); } +RobotOutputReceiver::RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance) { + model_instance_ = model_instance; + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + this->DeclareAbstractInputPort("lcmt_robot_output", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, u, t", + OutputVector(plant.num_positions(model_instance), + plant.num_velocities(model_instance), + plant.num_actuators()), + &RobotOutputReceiver::CopyOutput); +} + void RobotOutputReceiver::CopyOutput(const Context& context, OutputVector* output) const { const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); @@ -55,12 +85,12 @@ void RobotOutputReceiver::CopyOutput(const Context& context, VectorXd positions = VectorXd::Zero(num_positions_); for (int i = 0; i < state_msg.num_positions; i++) { int j = position_index_map_.at(state_msg.position_names[i]); - positions(j) = state_msg.position[i]; + positions(j - positions_start_idx_) = state_msg.position[i]; } VectorXd velocities = VectorXd::Zero(num_velocities_); for (int i = 0; i < state_msg.num_velocities; i++) { int j = velocity_index_map_.at(state_msg.velocity_names[i]); - velocities(j) = state_msg.velocity[i]; + velocities(j - velocities_start_idx_) = state_msg.velocity[i]; } VectorXd efforts = VectorXd::Zero(num_efforts_); for (int i = 0; i < state_msg.num_efforts; i++) { @@ -84,16 +114,18 @@ void RobotOutputReceiver::CopyOutput(const Context& context, void RobotOutputReceiver::InitializeSubscriberPositions( const MultibodyPlant& plant, - drake::systems::Context &context) const { + drake::systems::Context& context) const { auto& state_msg = context.get_mutable_abstract_state(0); // using the time from the context state_msg.utime = context.get_time() * 1e6; std::vector ordered_position_names = - multibody::ExtractOrderedNamesFromMap(position_index_map_); + multibody::ExtractOrderedNamesFromMap(position_index_map_, + positions_start_idx_); std::vector ordered_velocity_names = - multibody::ExtractOrderedNamesFromMap(velocity_index_map_); + multibody::ExtractOrderedNamesFromMap(velocity_index_map_, + velocities_start_idx_); std::vector ordered_effort_names = multibody::ExtractOrderedNamesFromMap(effort_index_map_); @@ -110,10 +142,16 @@ void RobotOutputReceiver::InitializeSubscriberPositions( } // Set quaternion w = 1, assumes drake quaternion ordering of wxyz - for (const auto& body_idx : plant.GetFloatingBaseBodies()) { - const auto& body = plant.get_body(body_idx); - if (body.has_quaternion_dofs()) { - state_msg.position[body.floating_positions_start()] = 1; + if (model_instance_ != drake::multibody::ModelInstanceIndex(-1)) { + if (plant.HasUniqueFreeBaseBody(model_instance_)) { + state_msg.position.at(0) = 1; + } + } else { + for (const auto& body_idx : plant.GetFloatingBaseBodies()) { + const auto& body = plant.get_body(body_idx); + if (body.has_quaternion_dofs()) { + state_msg.position.at(body.floating_positions_start()) = 1; + } } } @@ -125,7 +163,6 @@ void RobotOutputReceiver::InitializeSubscriberPositions( /*--------------------------------------------------------------------------*/ // methods implementation for RobotOutputSender. - RobotOutputSender::RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts, const bool publish_imu) @@ -138,6 +175,9 @@ RobotOutputSender::RobotOutputSender( velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap(position_index_map_); ordered_velocity_names_ = @@ -164,6 +204,54 @@ RobotOutputSender::RobotOutputSender( &RobotOutputSender::Output); } +RobotOutputSender::RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const bool publish_efforts, const bool publish_imu) + : publish_efforts_(publish_efforts), publish_imu_(publish_imu) { + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap( + position_index_map_, positions_start_idx_); + ordered_velocity_names_ = multibody::ExtractOrderedNamesFromMap( + velocity_index_map_, velocities_start_idx_); + ordered_effort_names_ = + multibody::ExtractOrderedNamesFromMap(effort_index_map_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + if (publish_efforts_) { + effort_input_port_ = + this->DeclareVectorInputPort("u", BasicVector(num_efforts_)) + .get_index(); + } + if (publish_imu_) { + imu_input_port_ = + this->DeclareVectorInputPort("imu_acceleration", BasicVector(3)) + .get_index(); + } + + this->DeclareAbstractOutputPort("lcmt_robot_output", + &RobotOutputSender::Output); +} + /// Populate a state message with all states void RobotOutputSender::Output(const Context& context, dairlib::lcmt_robot_output* state_msg) const { @@ -213,6 +301,213 @@ void RobotOutputSender::Output(const Context& context, } } +ObjectStateReceiver::ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant) { + num_positions_ = plant.num_positions(); + num_velocities_ = plant.num_velocities(); + position_index_map_ = multibody::MakeNameToPositionsMap(plant); + velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + model_instance_ = drake::multibody::ModelInstanceIndex(-1); + + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + this->DeclareAbstractInputPort("lcmt_object_state", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, t", + StateVector(plant.num_positions(), plant.num_velocities()), + &ObjectStateReceiver::CopyOutput); +} + +ObjectStateReceiver::ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance) { + model_instance_ = model_instance; + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + this->DeclareAbstractInputPort("lcmt_object_state", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, t", + StateVector(plant.num_positions(model_instance), + plant.num_velocities(model_instance)), + &ObjectStateReceiver::CopyOutput); +} + +void ObjectStateReceiver::CopyOutput(const Context& context, + StateVector* output) const { + const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); + DRAKE_ASSERT(input != nullptr); + const auto& state_msg = input->get_value(); + + VectorXd positions = VectorXd::Zero(num_positions_); + for (int i = 0; i < state_msg.num_positions; i++) { + int j = position_index_map_.at(state_msg.position_names[i]); + positions(j - positions_start_idx_) = state_msg.position[i]; + } + VectorXd velocities = VectorXd::Zero(num_velocities_); + for (int i = 0; i < state_msg.num_velocities; i++) { + int j = velocity_index_map_.at(state_msg.velocity_names[i]); + velocities(j - velocities_start_idx_) = state_msg.velocity[i]; + } + + output->SetPositions(positions); + output->SetVelocities(velocities); + output->set_timestamp(state_msg.utime * 1.0e-6); +} + +void ObjectStateReceiver::InitializeSubscriberPositions( + const MultibodyPlant& plant, + drake::systems::Context& context) const { + auto& state_msg = context.get_mutable_abstract_state(0); + + // using the time from the context + state_msg.utime = context.get_time() * 1e6; + + std::vector ordered_position_names = + multibody::ExtractOrderedNamesFromMap(position_index_map_, + positions_start_idx_); + std::vector ordered_velocity_names = + multibody::ExtractOrderedNamesFromMap(velocity_index_map_, + velocities_start_idx_); + + state_msg.num_positions = num_positions_; + state_msg.num_velocities = num_velocities_; + state_msg.position_names.resize(num_positions_); + state_msg.velocity_names.resize(num_velocities_); + state_msg.position.resize(num_positions_); + state_msg.velocity.resize(num_positions_); + + for (int i = 0; i < num_positions_; i++) { + state_msg.position_names[i] = ordered_position_names[i]; + state_msg.position[i] = 0; + } + + // Set quaternion w = 1, assumes drake quaternion ordering of wxyz + if (model_instance_ != drake::multibody::ModelInstanceIndex(-1)) { + if (plant.HasUniqueFreeBaseBody(model_instance_)) { + state_msg.position.at(0) = 1; + } + } else { + for (const auto& body_idx : plant.GetFloatingBaseBodies()) { + const auto& body = plant.get_body(body_idx); + if (body.has_quaternion_dofs()) { + state_msg.position.at(body.floating_positions_start()) = 1; + } + } + } + + for (int i = 0; i < num_velocities_; i++) { + state_msg.velocity[i] = 0; + state_msg.velocity_names[i] = ordered_velocity_names[i]; + } +} + +/*--------------------------------------------------------------------------*/ +// methods implementation for RobotOutputSender. +ObjectStateSender::ObjectStateSender( + const drake::multibody::MultibodyPlant& plant) { + num_positions_ = plant.num_positions(); + num_velocities_ = plant.num_velocities(); + + position_index_map_ = multibody::MakeNameToPositionsMap(plant); + velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + + model_instance_ = drake::multibody::ModelInstanceIndex(-1); + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + + ordered_position_names_ = + multibody::ExtractOrderedNamesFromMap(position_index_map_); + ordered_velocity_names_ = + multibody::ExtractOrderedNamesFromMap(velocity_index_map_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + + this->DeclareAbstractOutputPort("lcmt_object_state", + &ObjectStateSender::Output); +} + +ObjectStateSender::ObjectStateSender( + const drake::multibody::MultibodyPlant& plant, + bool publish_velocities, + drake::multibody::ModelInstanceIndex model_instance) + : publish_velocities_(publish_velocities), model_instance_(model_instance) { + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap( + position_index_map_, positions_start_idx_); + ordered_velocity_names_ = multibody::ExtractOrderedNamesFromMap( + velocity_index_map_, velocities_start_idx_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + + this->DeclareAbstractOutputPort("lcmt_object_state", + &ObjectStateSender::Output); +} + +/// Populate a state message with all states +void ObjectStateSender::Output(const Context& context, + dairlib::lcmt_object_state* state_msg) const { + const auto state = this->EvalVectorInput(context, state_input_port_); + + // using the time from the context + state_msg->utime = context.get_time() * 1e6; + + state_msg->num_positions = num_positions_; + state_msg->num_velocities = num_velocities_; + state_msg->position_names.resize(num_positions_); + state_msg->velocity_names.resize(num_velocities_); + state_msg->position.resize(num_positions_); + state_msg->velocity.resize(num_velocities_); + + for (int i = 0; i < num_positions_; i++) { + state_msg->position_names[i] = ordered_position_names_[i]; + if (std::isnan(state->GetAtIndex(i))) { + state_msg->position[i] = 0; + } else { + state_msg->position[i] = state->GetAtIndex(i); + } + } + for (int i = 0; i < num_velocities_; i++) { + state_msg->velocity[i] = 0; + if (publish_velocities_) { + state_msg->velocity[i] = state->GetAtIndex(num_positions_ + i); + } + state_msg->velocity_names[i] = ordered_velocity_names_[i]; + } +} + /*--------------------------------------------------------------------------*/ // methods implementation for RobotInputReceiver. @@ -222,9 +517,9 @@ RobotInputReceiver::RobotInputReceiver( actuator_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_input", drake::Value{}); - this->DeclareVectorOutputPort("u, t", - TimestampedVector(num_actuators_), - &RobotInputReceiver::CopyInputOut); + this->DeclareVectorOutputPort( + "u, t", TimestampedVector(num_actuators_), + &RobotInputReceiver::CopyInputOut, {all_sources_ticket()}); } void RobotInputReceiver::CopyInputOut(const Context& context, @@ -284,13 +579,10 @@ void RobotCommandSender::OutputCommand( SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts, - double actuator_delay) { - + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts, double actuator_delay) { // Create LCM input for actuators auto input_sub = builder->AddSystem(LcmSubscriberSystem::Make( @@ -307,31 +599,32 @@ SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( builder->AddSystem(LcmPublisherSystem::Make( state_channel, lcm, 1.0 / publish_rate)); auto state_sender = builder->AddSystem( - plant, publish_efforts); - builder->Connect(plant.get_state_output_port(), + plant, model_instance_index, publish_efforts); + + builder->Connect(plant.get_state_output_port(model_instance_index), state_sender->get_input_port_state()); // Add delay, if used, and associated connections if (actuator_delay > 0) { - auto discrete_time_delay = - builder->AddSystem( - 1.0 / publish_rate, actuator_delay * publish_rate, - plant.num_actuators()); - builder->Connect(*passthrough, *discrete_time_delay); + auto discrete_time_delay = + builder->AddSystem( + 1.0 / publish_rate, actuator_delay * publish_rate, + plant.num_actuators()); + builder->Connect(*passthrough, *discrete_time_delay); + builder->Connect(discrete_time_delay->get_output_port(), + plant.get_actuation_input_port()); + + if (publish_efforts) { builder->Connect(discrete_time_delay->get_output_port(), - plant.get_actuation_input_port()); - - if (publish_efforts) { - builder->Connect(discrete_time_delay->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } else { + builder->Connect(passthrough->get_output_port(), + plant.get_actuation_input_port()); + if (publish_efforts) { builder->Connect(passthrough->get_output_port(), - plant.get_actuation_input_port()); - if (publish_efforts) { - builder->Connect(passthrough->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } builder->Connect(*state_sender, *state_pub); diff --git a/systems/robot_lcm_systems.h b/systems/robot_lcm_systems.h index 3e1a88c0f6..7938d08d84 100644 --- a/systems/robot_lcm_systems.h +++ b/systems/robot_lcm_systems.h @@ -4,9 +4,12 @@ #include #include +#include + #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_robot_output.hpp" #include "systems/framework/output_vector.h" +#include "systems/framework/state_vector.h" #include "systems/framework/timestamped_vector.h" #include "systems/primitives/subvector_pass_through.h" @@ -18,10 +21,9 @@ namespace dairlib { namespace systems { /// @file This file contains classes dealing with sending/receiving -/// LCM messages related to a robot. The classes in this file are based on -/// acrobot_lcm.h +/// LCM messages related to a robot. -/// Receives the output of an LcmSubsriberSystem that subsribes to the +/// Receives the output of an LcmSubscriberSystem that subscribes to the /// Robot output channel with LCM type lcmt_robot_output, and outputs the /// robot states as a OutputVector. class RobotOutputReceiver : public drake::systems::LeafSystem { @@ -29,6 +31,10 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { explicit RobotOutputReceiver( const drake::multibody::MultibodyPlant& plant); + explicit RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance); + /// Convenience function to initialize an lcmt_robot_output subscriber with /// positions and velocities which are all zero except for the quaternion /// positions, which are all 1, 0, 0, 0 @@ -41,6 +47,9 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { private: void CopyOutput(const drake::systems::Context& context, OutputVector* output) const; + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -52,6 +61,11 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { /// Converts a OutputVector object to LCM type lcmt_robot_output class RobotOutputSender : public drake::systems::LeafSystem { public: + explicit RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index, + const bool publish_efforts = false, const bool publish_imu = false); + explicit RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts = false, const bool publish_imu = false); @@ -73,6 +87,8 @@ class RobotOutputSender : public drake::systems::LeafSystem { void Output(const drake::systems::Context& context, dairlib::lcmt_robot_output* output) const; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -89,7 +105,75 @@ class RobotOutputSender : public drake::systems::LeafSystem { bool publish_imu_; }; -/// Receives the output of an LcmSubsriberSystem that subsribes to the +/// @file This file contains classes dealing with sending/receiving +/// LCM messages related to a object. + +/// Receives the output of an LcmSubscriberSystem that subscribes to the +/// object state channel with LCM type lcmt_object_state, and outputs the +/// object state as a StateVector. +class ObjectStateReceiver : public drake::systems::LeafSystem { + public: + explicit ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant); + + explicit ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance); + + /// Convenience function to initialize an lcmt_object_state subscriber with + /// positions and velocities which are all zero except for the quaternion + /// positions, which are all 1, 0, 0, 0 + /// @param context The context of a + /// drake::LcmSubscriberSystem + void InitializeSubscriberPositions( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context) const; + + private: + void CopyOutput(const drake::systems::Context& context, + StateVector* output) const; + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; + int num_positions_; + int num_velocities_; + std::map position_index_map_; + std::map velocity_index_map_; +}; + +/// Converts a StateVector object to LCM type lcmt_robot_output +class ObjectStateSender : public drake::systems::LeafSystem { + public: + explicit ObjectStateSender( + const drake::multibody::MultibodyPlant& plant, bool publish_velocities = true, + drake::multibody::ModelInstanceIndex model_instance_index = + drake::multibody::default_model_instance()); + + explicit ObjectStateSender( + const drake::multibody::MultibodyPlant& plant); + + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_input_port_); + } + + private: + void Output(const drake::systems::Context& context, + dairlib::lcmt_object_state* output) const; + + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; + int num_positions_; + int num_velocities_; + std::vector ordered_position_names_; + std::vector ordered_velocity_names_; + std::map position_index_map_; + std::map velocity_index_map_; + drake::systems::InputPortIndex state_input_port_; + bool publish_velocities_; +}; + +/// Receives the output of an LcmSubscriberSystem that subscribes to the /// robot input channel with LCM type lcmt_robot_input and outputs the /// robot inputs as a TimestampedVector. class RobotInputReceiver : public drake::systems::LeafSystem { @@ -122,7 +206,6 @@ class RobotCommandSender : public drake::systems::LeafSystem { std::map actuator_index_map_; }; - /// /// Convenience method to add and connect leaf systems for controlling /// a MultibodyPlant via LCM. Makes two primary connections: @@ -148,11 +231,10 @@ class RobotCommandSender : public drake::systems::LeafSystem { SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const drake::multibody::MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts = true, - double actuator_delay = 0); + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts = true, double actuator_delay = 0); + } // namespace systems } // namespace dairlib diff --git a/systems/trajectory_optimization/BUILD.bazel b/systems/trajectory_optimization/BUILD.bazel index 0926507e40..a8bcc809ff 100644 --- a/systems/trajectory_optimization/BUILD.bazel +++ b/systems/trajectory_optimization/BUILD.bazel @@ -45,6 +45,39 @@ cc_library( ], ) +cc_library( + name = "lcm_trajectory_systems", + srcs = [ + "lcm_trajectory_systems.cc", + ], + hdrs = [ + "lcm_trajectory_systems.h", + ], + deps = [ + "//common:eigen_utils", + "//common:find_resource", + "//lcm:lcm_trajectory_saver", + "//multibody:multipose_visualizer", + "@drake//:drake_shared_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 new file mode 100644 index 0000000000..61691633e3 --- /dev/null +++ b/systems/trajectory_optimization/c3_output_systems.cc @@ -0,0 +1,111 @@ +#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 new file mode 100644 index 0000000000..1975721240 --- /dev/null +++ b/systems/trajectory_optimization/c3_output_systems.h @@ -0,0 +1,67 @@ +#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/trajectory_optimization/lcm_trajectory_systems.cc b/systems/trajectory_optimization/lcm_trajectory_systems.cc new file mode 100644 index 0000000000..6b097023dd --- /dev/null +++ b/systems/trajectory_optimization/lcm_trajectory_systems.cc @@ -0,0 +1,133 @@ +#include "lcm_trajectory_systems.h" + +#include + + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" + +#include "drake/common/schema/rotation.h" +#include "drake/geometry/rgba.h" + +namespace dairlib { +namespace systems { + +using drake::geometry::Rgba; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; + +LcmTrajectoryReceiver::LcmTrajectoryReceiver(std::string trajectory_name) + : trajectory_name_(std::move(trajectory_name)) { + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + PiecewisePolynomial empty_pp_traj(Eigen::VectorXd(0)); + Trajectory& traj_inst = empty_pp_traj; + this->set_name(trajectory_name_); + trajectory_output_port_ = + this->DeclareAbstractOutputPort(trajectory_name_, traj_inst, + &LcmTrajectoryReceiver::OutputTrajectory) + .get_index(); +} + +void LcmTrajectoryReceiver::OutputTrajectory( + const drake::systems::Context& context, + Trajectory* traj) const { + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime > 1e-3) { + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + if (trajectory_block.datapoints.rows() == 3) { + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + } else { + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints.topRows(trajectory_block.datapoints.rows() / 2)); +// *casted_traj = PiecewisePolynomial::CubicHermite( +// trajectory_block.time_vector, trajectory_block.datapoints.topRows(3), +// trajectory_block.datapoints.bottomRows(3)); + } + } else { + *casted_traj = PiecewisePolynomial(Vector3d::Zero()); + } +} + +LcmOrientationTrajectoryReceiver::LcmOrientationTrajectoryReceiver( + std::string trajectory_name) + : trajectory_name_(std::move(trajectory_name)) { + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + PiecewiseQuaternionSlerp empty_slerp_traj; + Trajectory& traj_inst = empty_slerp_traj; + this->set_name(trajectory_name_); + trajectory_output_port_ = + this->DeclareAbstractOutputPort( + trajectory_name_, traj_inst, + &LcmOrientationTrajectoryReceiver::OutputTrajectory) + .get_index(); +} + +void LcmOrientationTrajectoryReceiver::OutputTrajectory( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + auto* casted_traj = (PiecewiseQuaternionSlerp*)dynamic_cast< + PiecewiseQuaternionSlerp*>(traj); + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime > 1e-3) { + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + try { + lcm_traj.GetTrajectory(trajectory_name_); + } catch (std::exception& e) { + std::cerr << "Make sure the planner is sending orientation" << std::endl; + throw std::out_of_range(""); + } + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + + std::vector> quaternion_datapoints; + for (int i = 0; i < trajectory_block.datapoints.cols(); ++i) { + quaternion_datapoints.push_back( + drake::math::RollPitchYaw(trajectory_block.datapoints.col(i)) + .ToQuaternion()); + } + *casted_traj = PiecewiseQuaternionSlerp( + CopyVectorXdToStdVector(trajectory_block.time_vector), + quaternion_datapoints); + } else { + *casted_traj = drake::trajectories::PiecewiseQuaternionSlerp( + {0, 1}, + {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); + } +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/trajectory_optimization/lcm_trajectory_systems.h b/systems/trajectory_optimization/lcm_trajectory_systems.h new file mode 100644 index 0000000000..bc46dc988e --- /dev/null +++ b/systems/trajectory_optimization/lcm_trajectory_systems.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "dairlib/lcmt_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "multibody/multipose_visualizer.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and outputs it as a drake PiecewisePolynomial. +class LcmTrajectoryReceiver : public drake::systems::LeafSystem { + public: + explicit LcmTrajectoryReceiver(std::string trajectory_name); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + const drake::systems::OutputPort& get_output_port_trajectory() const { + return this->get_output_port(trajectory_output_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::InputPortIndex trajectory_input_port_; + drake::systems::OutputPortIndex trajectory_output_port_; + const std::string trajectory_name_; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and outputs it as a drake PiecewisePolynomial. +class LcmOrientationTrajectoryReceiver + : public drake::systems::LeafSystem { + public: + explicit LcmOrientationTrajectoryReceiver(std::string trajectory_name); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + const drake::systems::OutputPort& get_output_port_trajectory() const { + return this->get_output_port(trajectory_output_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + drake::systems::InputPortIndex trajectory_input_port_; + drake::systems::OutputPortIndex trajectory_output_port_; + const std::string trajectory_name_; +}; + + +} // namespace systems +} // namespace dairlib diff --git a/systems/visualization/BUILD.bazel b/systems/visualization/BUILD.bazel new file mode 100644 index 0000000000..34d0ffcff7 --- /dev/null +++ b/systems/visualization/BUILD.bazel @@ -0,0 +1,32 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "lcm_visualization_systems", + srcs = [ + "lcm_visualization_systems.cc", + ], + hdrs = [ + "lcm_visualization_systems.h", + ], + deps = [ + "//common:eigen_utils", + "//common:find_resource", + "//lcm:lcm_trajectory_saver", + "//multibody:multipose_visualizer", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "meshcat_dynamic_camera", + srcs = ["meshcat_dynamic_camera.cc"], + hdrs = ["meshcat_dynamic_camera.h"], + deps = [ + "//common", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) diff --git a/systems/visualization/lcm_visualization_systems.cc b/systems/visualization/lcm_visualization_systems.cc new file mode 100644 index 0000000000..e879ad3249 --- /dev/null +++ b/systems/visualization/lcm_visualization_systems.cc @@ -0,0 +1,479 @@ +#include "lcm_visualization_systems.h" + +#include +#include +#include + +#include "common/eigen_utils.h" + +#include "drake/common/schema/rotation.h" +#include "drake/geometry/rgba.h" + +namespace dairlib { +namespace systems { + +using drake::geometry::Rgba; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; + +LcmTrajectoryDrawer::LcmTrajectoryDrawer( + const std::shared_ptr& meshcat, + std::string trajectory_name) + : meshcat_(meshcat), trajectory_name_(std::move(trajectory_name)) { + this->set_name("LcmTrajectoryDrawer: " + trajectory_name_); + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + DeclarePerStepDiscreteUpdateEvent(&LcmTrajectoryDrawer::DrawTrajectory); +} + +drake::systems::EventStatus LcmTrajectoryDrawer::DrawTrajectory( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + MatrixXd line_points = MatrixXd::Zero(3, N_); + VectorXd breaks = + VectorXd::LinSpaced(N_, trajectory_block.time_vector[0], + trajectory_block.time_vector.tail(1)[0]); + if (trajectory_block.datapoints.rows() == 3) { + auto trajectory = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + for (int i = 0; i < line_points.cols(); ++i) { + line_points.col(i) = trajectory.value(breaks(i)); + } + } else { + auto trajectory = PiecewisePolynomial::CubicHermite( + trajectory_block.time_vector, trajectory_block.datapoints.topRows(3), + trajectory_block.datapoints.bottomRows(3)); + for (int i = 0; i < line_points.cols(); ++i) { + line_points.col(i) = trajectory.value(breaks(i)); + } + } + + DRAKE_DEMAND(line_points.rows() == 3); + meshcat_->SetLine("/trajectories/" + trajectory_name_, line_points, 100, + rgba_); + return drake::systems::EventStatus::Succeeded(); +} + +LcmPoseDrawer::LcmPoseDrawer( + const std::shared_ptr& meshcat, + const std::string& model_file, + const std::string& translation_trajectory_name, + const std::string& orientation_trajectory_name, int num_poses, + bool add_transparency) + : meshcat_(meshcat), + translation_trajectory_name_(translation_trajectory_name), + orientation_trajectory_name_(orientation_trajectory_name), + N_(num_poses) { + this->set_name("LcmPoseDrawer: " + translation_trajectory_name); + + Eigen::VectorXd alpha_scale; + if (add_transparency) { + alpha_scale = 1.0 * VectorXd::LinSpaced(N_ - 1, 0.1, 0.4); + } else { + alpha_scale = 1.0 * VectorXd::Ones(N_ - 1); + } + + multipose_visualizer_ = std::make_unique( + model_file, N_ - 1, alpha_scale, "", meshcat); + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + DeclarePerStepDiscreteUpdateEvent(&LcmPoseDrawer::DrawTrajectory); +} + +drake::systems::EventStatus LcmPoseDrawer::DrawTrajectory( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + MatrixXd object_poses = MatrixXd::Zero(7, N_ - 1); + + const auto& lcm_translation_traj = + lcm_traj.GetTrajectory(translation_trajectory_name_); + auto translation_trajectory = PiecewisePolynomial::CubicHermite( + lcm_translation_traj.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)}); + + if (lcm_traj.HasTrajectory(orientation_trajectory_name_)) { + const auto& lcm_orientation_traj = + lcm_traj.GetTrajectory(orientation_trajectory_name_); + std::vector> quaternion_datapoints; + for (int i = 0; i < lcm_orientation_traj.datapoints.cols(); ++i) { + VectorXd orientation_sample = lcm_orientation_traj.datapoints.col(i); + if (orientation_sample.isZero()) { + quaternion_datapoints.push_back(Quaterniond(1, 0, 0, 0)); + } else { + quaternion_datapoints.push_back( + Quaterniond(orientation_sample[0], orientation_sample[1], + orientation_sample[2], orientation_sample[3])); + } + } + orientation_trajectory = PiecewiseQuaternionSlerp( + CopyVectorXdToStdVector(lcm_orientation_traj.time_vector), + quaternion_datapoints); + } + + // ASSUMING orientation and translation trajectories have the same breaks + VectorXd translation_breaks = + VectorXd::LinSpaced(N_, lcm_translation_traj.time_vector[0], + lcm_translation_traj.time_vector.tail(1)[0]); + for (int i = 0; i < object_poses.cols(); ++i) { + object_poses.col(i) << orientation_trajectory.value( + translation_breaks(i + 1)), + translation_trajectory.value(translation_breaks(i + 1)); + } + + multipose_visualizer_->DrawPoses(object_poses); + + return drake::systems::EventStatus::Succeeded(); +} + +LcmForceDrawer::LcmForceDrawer( + const std::shared_ptr& meshcat, + std::string actor_trajectory_name, std::string force_trajectory_name, + std::string lcs_force_trajectory_name) + : meshcat_(meshcat), + actor_trajectory_name_(std::move(actor_trajectory_name)), + force_trajectory_name_(std::move(force_trajectory_name)), + lcs_force_trajectory_name_(std::move(lcs_force_trajectory_name)) { + this->set_name("LcmForceDrawer: " + force_trajectory_name_); + actor_trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj: actor", + drake::Value{}) + .get_index(); + + robot_time_input_port_ = + this->DeclareVectorInputPort("t_robot", 1).get_index(); + + force_trajectory_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_forces", + drake::Value{}) + .get_index(); + + meshcat_->SetProperty(force_path_, "visible", true, 0); + + actor_last_update_time_index_ = this->DeclareDiscreteState(1); + forces_last_update_time_index_ = this->DeclareDiscreteState(1); + meshcat_->SetObject(force_path_ + "/u_lcs/arrow/cylinder", cylinder_, + actor_force_color_); + meshcat_->SetObject(force_path_ + "/u_lcs/arrow/head", arrowhead_, + actor_force_color_); + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false); + + DeclarePerStepDiscreteUpdateEvent(&LcmForceDrawer::DrawForce); + DeclarePerStepDiscreteUpdateEvent(&LcmForceDrawer::DrawForces); +} + +drake::systems::EventStatus LcmForceDrawer::DrawForce( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, actor_trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, actor_trajectory_input_port_); + + // Don't needlessly update + if (discrete_state->get_value(actor_last_update_time_index_)[0] == + lcmt_traj->utime * 1e-6) { + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(actor_last_update_time_index_)[0] = + lcmt_traj->utime * 1e-6; + const auto& robot_time_vec = + this->EvalVectorInput(context, robot_time_input_port_); + double robot_time = robot_time_vec->GetAtIndex(0); + + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& force_trajectory_block = + lcm_traj.GetTrajectory(force_trajectory_name_); + const auto& actor_trajectory_block = + lcm_traj.GetTrajectory(actor_trajectory_name_); + auto force_trajectory = PiecewisePolynomial::FirstOrderHold( + force_trajectory_block.time_vector, force_trajectory_block.datapoints); + VectorXd pose; + if (actor_trajectory_block.datapoints.rows() == 3) { + auto trajectory = PiecewisePolynomial::FirstOrderHold( + actor_trajectory_block.time_vector, actor_trajectory_block.datapoints); + pose = trajectory.value(robot_time); + } else { + auto trajectory = PiecewisePolynomial::CubicHermite( + actor_trajectory_block.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()); + const std::string& force_arrow_path = force_path_root + "arrow"; + + auto force_norm = force.norm(); + // Stretch the cylinder in z. + if (force_norm >= 0.01) { + meshcat_->SetTransform( + force_arrow_path, + 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()); + // Note: Meshcat does not fully support non-uniform scaling (see #18095). + // We get away with it here since there is no rotation on this frame and + // no children in the kinematic tree. + meshcat_->SetProperty(force_arrow_path + "/cylinder", "scale", + {1, 1, height}, context.get_time()); + // Translate the arrowheads. + const double arrowhead_height = radius_ * 2.0; + 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()); + } else { + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false, context.get_time()); + } + return drake::systems::EventStatus::Succeeded(); +} + +drake::systems::EventStatus LcmForceDrawer::DrawForces( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, force_trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& c3_forces = this->EvalInputValue( + context, force_trajectory_input_port_); + + // Don't needlessly update + if (discrete_state->get_value(forces_last_update_time_index_)[0] == + c3_forces->utime * 1e-6) { + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(forces_last_update_time_index_)[0] = + c3_forces->utime * 1e-6; + + for (int i = 0; i < c3_forces->num_forces; ++i) { + const VectorXd force = Eigen::Map( + c3_forces->forces[i].contact_force, 3); + auto force_norm = force.norm(); + const std::string& force_path_root = + force_path_ + "/lcs_force_" + std::to_string(i) + "/"; + if (force_norm >= 0.5) { + if (!meshcat_->HasPath(force_path_root + "arrow/")) { + meshcat_->SetObject(force_path_root + "arrow/cylinder", cylinder_, + contact_force_color_); + meshcat_->SetObject(force_path_root + "arrow/head", arrowhead_, + contact_force_color_); + } + + const VectorXd pose = Eigen::Map( + c3_forces->forces[i].contact_point, 3); + + 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()); + const double height = force_norm / newtons_per_meter_; + meshcat_->SetProperty(force_arrow_path + "/cylinder", "position", + {0, 0, 0.5 * height}, context.get_time()); + // Note: Meshcat does not fully support non-uniform scaling (see + // #18095). We get away with it here since there is no rotation on this + // frame and no children in the kinematic tree. + meshcat_->SetProperty(force_arrow_path + "/cylinder", "scale", + {1, 1, height}, context.get_time()); + // Translate the arrowheads. + const double arrowhead_height = radius_ * 2.0; + 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()); + } else { + meshcat_->SetProperty(force_path_root, "visible", false, context.get_time()); + } + } + return drake::systems::EventStatus::Succeeded(); +} + +LcmC3TargetDrawer::LcmC3TargetDrawer( + const std::shared_ptr& meshcat, bool draw_tray, + bool draw_ee) + : meshcat_(meshcat), draw_tray_(draw_tray), draw_ee_(draw_ee) { + this->set_name("LcmC3TargetDrawer"); + c3_state_target_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_state: target", + drake::Value{}) + .get_index(); + + c3_state_actual_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_state: actual", + drake::Value{}) + .get_index(); + last_update_time_index_ = this->DeclareDiscreteState(1); + + meshcat_->SetProperty(c3_state_path_, "visible", true, 0); + + // TODO(yangwill): Clean up all this visualization, move to separate + // visualization directory1 + meshcat_->SetObject(c3_target_object_path_ + "/x-axis", cylinder_for_tray_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_target_object_path_ + "/y-axis", cylinder_for_tray_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_target_object_path_ + "/z-axis", cylinder_for_tray_, + {0, 0, 1, 1}); + meshcat_->SetObject(c3_actual_object_path_ + "/x-axis", cylinder_for_tray_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_actual_object_path_ + "/y-axis", cylinder_for_tray_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_actual_object_path_ + "/z-axis", cylinder_for_tray_, + {0, 0, 1, 1}); + 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_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_target_ee_path_ + "/z-axis", cylinder_for_ee_, + {0, 0, 1, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/x-axis", cylinder_for_ee_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/y-axis", cylinder_for_ee_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/z-axis", cylinder_for_ee_, + {0, 0, 1, 1}); + } + auto x_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitY()), + Vector3d{0.05, 0.0, 0.0}); + auto y_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitX()), + Vector3d{0.0, 0.05, 0.0}); + auto z_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitZ()), + Vector3d{0.0, 0.0, 0.05}); + auto x_axis_transform_ee = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitY()), + 0.5 * Vector3d{0.05, 0.0, 0.0}); + auto y_axis_transform_ee = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitX()), + 0.5 * Vector3d{0.0, 0.05, 0.0}); + 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_target_object_path_ + "/x-axis", x_axis_transform, 0.0); + meshcat_->SetTransform(c3_target_object_path_ + "/y-axis", y_axis_transform, 0.0); + meshcat_->SetTransform(c3_target_object_path_ + "/z-axis", z_axis_transform, 0.0); + meshcat_->SetTransform(c3_actual_object_path_ + "/x-axis", x_axis_transform, 0.0); + meshcat_->SetTransform(c3_actual_object_path_ + "/y-axis", y_axis_transform, 0.0); + meshcat_->SetTransform(c3_actual_object_path_ + "/z-axis", z_axis_transform, 0.0); + if (draw_ee_){ + meshcat_->SetTransform(c3_target_ee_path_ + "/x-axis", x_axis_transform_ee, 0.0); + meshcat_->SetTransform(c3_target_ee_path_ + "/y-axis", y_axis_transform_ee, 0.0); + meshcat_->SetTransform(c3_target_ee_path_ + "/z-axis", z_axis_transform_ee, 0.0); + meshcat_->SetTransform(c3_actual_ee_path_ + "/x-axis", x_axis_transform_ee, 0.0); + meshcat_->SetTransform(c3_actual_ee_path_ + "/y-axis", y_axis_transform_ee, 0.0); + meshcat_->SetTransform(c3_actual_ee_path_ + "/z-axis", z_axis_transform_ee, 0.0); + } + + DeclarePerStepDiscreteUpdateEvent(&LcmC3TargetDrawer::DrawC3State); +} + +drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue(context, + c3_state_target_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + if (this->EvalInputValue(context, + c3_state_actual_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + if (discrete_state->get_value(last_update_time_index_)[0] >= + context.get_time()) { + // no need to update if simulation has not advanced + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(last_update_time_index_)[0] = + context.get_time(); + const auto& c3_target = this->EvalInputValue( + context, c3_state_target_input_port_); + const auto& c3_actual = this->EvalInputValue( + context, c3_state_actual_input_port_); + if (draw_tray_) { + meshcat_->SetTransform( + c3_target_object_path_, + RigidTransformd( + 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()); + 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()); + } + if (draw_ee_) { + meshcat_->SetTransform( + c3_target_ee_path_, + RigidTransformd(Vector3d{c3_target->state[0], c3_target->state[1], + c3_target->state[2]}), context.get_time()); + meshcat_->SetTransform( + c3_actual_ee_path_, + RigidTransformd(Vector3d{c3_actual->state[0], c3_actual->state[1], + c3_actual->state[2]}), context.get_time()); + } + return drake::systems::EventStatus::Succeeded(); +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/visualization/lcm_visualization_systems.h b/systems/visualization/lcm_visualization_systems.h new file mode 100644 index 0000000000..aca1c8642e --- /dev/null +++ b/systems/visualization/lcm_visualization_systems.h @@ -0,0 +1,180 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "dairlib/lcmt_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "multibody/multipose_visualizer.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmTrajectoryDrawer : public drake::systems::LeafSystem { + public: + explicit LcmTrajectoryDrawer(const std::shared_ptr&, + std::string trajectory_name); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + void SetLineColor(drake::geometry::Rgba rgba) { rgba_ = rgba; } + + void SetNumSamples(int N) { + DRAKE_DEMAND(N > 1); + N_ = N; + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::EventStatus DrawTrajectory( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex trajectory_input_port_; + std::shared_ptr meshcat_; + const std::string trajectory_name_; + drake::geometry::Rgba rgba_ = drake::geometry::Rgba(0.1, 0.1, 0.1, 1.0); + int N_ = 5; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws the object pose through meshcat. +class LcmPoseDrawer : public drake::systems::LeafSystem { + public: + explicit LcmPoseDrawer(const std::shared_ptr&, + const std::string& model_file, + const std::string& translation_trajectory_name, + const std::string& orientation_trajectory_name, + int num_poses = 5, + bool add_transparency = true); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::EventStatus DrawTrajectory( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex trajectory_input_port_; + std::shared_ptr meshcat_; + const std::string translation_trajectory_name_; + const std::string orientation_trajectory_name_; + std::unique_ptr multipose_visualizer_; + const int N_; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmForceDrawer : public drake::systems::LeafSystem { + public: + explicit LcmForceDrawer(const std::shared_ptr&, + std::string force_trajectory_name, + std::string actor_trajectory_name, + std::string lcs_force_trajectory_name); + + const drake::systems::InputPort& get_input_port_actor_trajectory() + const { + return this->get_input_port(actor_trajectory_input_port_); + } + + const drake::systems::InputPort& get_input_port_robot_time() const { + return this->get_input_port(robot_time_input_port_); + } + + const drake::systems::InputPort& get_input_port_force_trajectory() + const { + return this->get_input_port(force_trajectory_input_port_); + } + + private: + drake::systems::EventStatus DrawForce( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + drake::systems::EventStatus DrawForces( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex actor_trajectory_input_port_; + drake::systems::InputPortIndex robot_time_input_port_; + drake::systems::InputPortIndex force_trajectory_input_port_; + + drake::systems::DiscreteStateIndex actor_last_update_time_index_; + drake::systems::DiscreteStateIndex forces_last_update_time_index_; + std::shared_ptr meshcat_; + const drake::geometry::Cylinder cylinder_ = + drake::geometry::Cylinder(0.002, 1.0); + const drake::geometry::MeshcatCone arrowhead_ = + drake::geometry::MeshcatCone(0.004, 0.004, 0.004); + const std::string force_path_ = "c3_forces"; + const std::string actor_trajectory_name_; + const std::string force_trajectory_name_; + const std::string lcs_force_trajectory_name_; + drake::geometry::Rgba actor_force_color_ = drake::geometry::Rgba(1, 0, 1, 1.0); + drake::geometry::Rgba contact_force_color_ = drake::geometry::Rgba(0.949, 0.757, 0.0, 1.0); + const double radius_ = 0.002; + const double newtons_per_meter_ = 40; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmC3TargetDrawer : public drake::systems::LeafSystem { + public: + explicit LcmC3TargetDrawer(const std::shared_ptr&, + bool draw_tray = true, bool draw_ee = false); + + const drake::systems::InputPort& get_input_port_c3_state_target() + const { + return this->get_input_port(c3_state_target_input_port_); + } + + const drake::systems::InputPort& get_input_port_c3_state_actual() + const { + return this->get_input_port(c3_state_actual_input_port_); + } + + private: + drake::systems::EventStatus DrawC3State( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + std::shared_ptr meshcat_; + + drake::systems::InputPortIndex c3_state_target_input_port_; + drake::systems::InputPortIndex c3_state_actual_input_port_; + + bool draw_tray_; + bool draw_ee_; + + drake::systems::DiscreteStateIndex last_update_time_index_; + + const drake::geometry::Cylinder cylinder_for_tray_ = + drake::geometry::Cylinder(0.005, 0.1); + const drake::geometry::Cylinder cylinder_for_ee_ = + drake::geometry::Cylinder(0.0025, 0.05); + const std::string c3_state_path_ = "c3_state"; + const std::string c3_target_object_path_ = "c3_state/c3_target_object"; + const std::string c3_actual_object_path_ = "c3_state/c3_actual_object"; + const std::string c3_target_ee_path_ = "c3_state/c3_target_ee"; + const std::string c3_actual_ee_path_ = "c3_state/c3_actual_ee"; +}; +} // namespace systems +} // namespace dairlib diff --git a/systems/visualization/meshcat_dynamic_camera.cc b/systems/visualization/meshcat_dynamic_camera.cc new file mode 100644 index 0000000000..12570f6d18 --- /dev/null +++ b/systems/visualization/meshcat_dynamic_camera.cc @@ -0,0 +1,45 @@ +#include "systems/visualization/meshcat_dynamic_camera.h" + +#include + +#include "systems/framework/output_vector.h" + +namespace dairlib { +using systems::OutputVector; + +MeshcatDynamicCamera::MeshcatDynamicCamera( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* plant_context, + const std::shared_ptr& meshcat, + const drake::multibody::RigidBodyFrame& body_frame_to_track) + : plant_(plant), + plant_context_(plant_context), + body_frame_to_track_(body_frame_to_track), + meshcat_(meshcat) { + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(plant.num_positions(), + plant.num_velocities(), + plant.num_actuators())) + .get_index(); + DeclarePerStepDiscreteUpdateEvent(&MeshcatDynamicCamera::UpdateMeshcat); +} + +drake::systems::EventStatus MeshcatDynamicCamera::UpdateMeshcat( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const { + const OutputVector* robot_output = + (OutputVector*)this->EvalVectorInput(context, state_port_); + plant_.SetPositions(plant_context_, robot_output->GetPositions()); + Eigen::VectorXd q = robot_output->GetPositions(); + Eigen::VectorXd body_pos_in_world = Eigen::VectorXd::Zero(3); + plant_.CalcPointsPositions(*plant_context_, body_frame_to_track_, + Eigen::VectorXd::Zero(3), plant_.world_frame(), + &body_pos_in_world); + body_pos_in_world[2] = 0.8; + Eigen::VectorXd camera_pos_offset = Eigen::VectorXd::Zero(3); + camera_pos_offset << 0.1, 3.0, 0.5; + meshcat_->SetCameraPose(body_pos_in_world + camera_pos_offset, body_pos_in_world); + return drake::systems::EventStatus::Succeeded(); +} + +} // namespace dairlib \ No newline at end of file diff --git a/systems/visualization/meshcat_dynamic_camera.h b/systems/visualization/meshcat_dynamic_camera.h new file mode 100644 index 0000000000..8daeef7db5 --- /dev/null +++ b/systems/visualization/meshcat_dynamic_camera.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class MeshcatDynamicCamera : public drake::systems::LeafSystem { + public: + explicit MeshcatDynamicCamera( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* plant_context, + const std::shared_ptr&, + const drake::multibody::RigidBodyFrame& body_frame_to_track); + + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_port_); + } + + private: + drake::systems::EventStatus UpdateMeshcat( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex state_port_; + // Kinematic calculations + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context* plant_context_; + const drake::multibody::RigidBodyFrame& body_frame_to_track_; + + std::shared_ptr meshcat_; +}; + +} // namespace dairlib \ No newline at end of file