diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt new file mode 100644 index 000000000..74d603fa1 --- /dev/null +++ b/example_XX/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_xx LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + backward_ros + pluginlib + generate_parameter_library + joint_trajectory_controller_plugins + trajectory_msgs + rclcpp + rclpy +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(cartpole_lqr_trajectory_plugin_parameters + controller/cartpole_lqr_trajectory_plugin_parameters.yaml +) +add_library(${PROJECT_NAME} SHARED controller/cartpole_lqr_trajectory_plugin.cpp) +add_library(ros2_control_demo_example_xx::cartpole_lqr_trajectory_plugin ALIAS ${PROJECT_NAME}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} PUBLIC + cartpole_lqr_trajectory_plugin_parameters +) +pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_XX_BUILDING_LIBRARY") + +# INSTALL +install( + DIRECTORY description/launch description/ros2_control description/urdf description/gazebo + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/${PROJECT_NAME} +) +install( + TARGETS ${PROJECT_NAME} cartpole_lqr_trajectory_plugin_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install(PROGRAMS + scripts/trajectory_action_client.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_libraries( + cartpole_lqr_trajectory_plugin +) +ament_export_targets( + export_${PROJECT_NAME} +) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_XX/README.md b/example_XX/README.md new file mode 100644 index 000000000..50a82a426 --- /dev/null +++ b/example_XX/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_xx + + This example demonstrates a robot with passive joints. + +Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_XX/doc/userdoc.html). diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml new file mode 100644 index 000000000..d90a012c0 --- /dev/null +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -0,0 +1,63 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + trajectory_controllers: + type: joint_trajectory_controller/JointTrajectoryController + + trajectory_controllers_pid: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +trajectory_controllers: + ros__parameters: + + joints: + - slider_to_cart + - cart_to_pendulum + + command_joints: + - slider_to_cart + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + controller_plugin: ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin + + gains: + R: 0.1 + Q: [5.0, 5.0, 0.1, 1.0] + cart_to_pendulum: + angle_wraparound: true + +trajectory_controllers_pid: + ros__parameters: + joints: + - slider_to_cart + - cart_to_pendulum + + command_joints: + - slider_to_cart + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + gains: + slider_to_cart: + p: 1.0 + i: 0.0 + d: 0.0 + i_clamp: 0.0 + ff_velocity_scale: 1.0 + cart_to_pendulum: + angle_wraparound: true diff --git a/example_XX/bringup/config/plotjuggler.xml b/example_XX/bringup/config/plotjuggler.xml new file mode 100644 index 000000000..1a8dad5f0 --- /dev/null +++ b/example_XX/bringup/config/plotjuggler.xml @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_XX/bringup/launch/cart_pole.launch.py b/example_XX/bringup/launch/cart_pole.launch.py new file mode 100644 index 000000000..6248abadb --- /dev/null +++ b/example_XX/bringup/launch/cart_pole.launch.py @@ -0,0 +1,107 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("gazebo_ros"), "launch"), + "/gazebo.launch.py", + ] + ), + launch_arguments={"pause": "false"}.items(), + ) + + ros2_control_demo_example_xx_path = os.path.join( + get_package_share_directory("ros2_control_demo_example_xx") + ) + + xacro_file = os.path.join(ros2_control_demo_example_xx_path, "urdf", "cart_pole.urdf.xacro") + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {"robot_description": doc.toxml()} + + node_robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[params], + ) + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "robot_description", "-entity", "cartpole"], + output="screen", + ) + + load_joint_state_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "trajectory_controllers", + ], + output="screen", + ) + + return LaunchDescription( + [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ] + ) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp new file mode 100644 index 000000000..39debc64a --- /dev/null +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -0,0 +1,382 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp" + +namespace ros2_control_demo_example_xx +{ + +/* ----- TrajectoryLQR ----- */ + +MatrixUX TrajectoryLQR::get_feedback_gain(const rclcpp::Duration & duration_since_start) const +{ + // TODO(christophfroehlich) interpolate K from time + auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); + int idx; + if (it == time_vec_.end()) + { + idx = time_vec_.size() - 1; + } + else + { + idx = std::distance(time_vec_.begin(), it); + } + return K_vec_.at(idx); +} + +/* ----- CartpoleLqrTrajectoryPlugin ----- */ + +bool CartpoleLqrTrajectoryPlugin::initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector map_cmd_to_joints) +{ + node_ = node; + map_cmd_to_joints_ = map_cmd_to_joints; + if (1 != map_cmd_to_joints_.size()) + { + RCLCPP_ERROR( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joints has to have size 1."); + return false; + } + for (const auto & map_cmd_to_joint : map_cmd_to_joints_) + { + RCLCPP_INFO( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joint: %lu", map_cmd_to_joint); + } + + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(node_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + trajectory_active_lqr_ptr_ = std::make_shared(); + + return true; +} + +bool CartpoleLqrTrajectoryPlugin::configure() +{ + try + { + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return false; + } + + // parse read-only params + num_cmd_joints_ = params_.command_joints.size(); + if (num_cmd_joints_ == 0) + { + RCLCPP_ERROR(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] No command joints specified."); + return false; + } + if (num_cmd_joints_ != map_cmd_to_joints_.size()) + { + RCLCPP_ERROR( + node_->get_logger(), + "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints."); + return false; + } + + reset(); + + return true; +} + +bool CartpoleLqrTrajectoryPlugin::activate() +{ + params_ = param_listener_->get_params(); + parseGains(); + return true; +} + +VectorX get_state_from_point(trajectory_msgs::msg::JointTrajectoryPoint point) +{ + // TODO(anyone) create map to get the correct joint order + // theta, theta_dot, x, x_dot + if (point.positions.size() != 2) + { + throw std::runtime_error("JointTrajectoryPoint.positions does not have the correct size."); + } + if (point.velocities.size() != 2) + { + throw std::runtime_error("JointTrajectoryPoint.velocities does not have the correct size."); + } + return VectorX{point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; +} + +bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( + const std::shared_ptr & trajectory) +{ + auto start_time = this->node_->now(); + + // parameters + auto N = MatrixUX::Zero(); + + MatrixUX Ks; + MatrixXX Ps; + auto x_R = get_state_from_point(trajectory->points.back()); + VectorU u_R{0}; + calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); + + auto new_trajectory_gains = std::make_shared(); + + if (trajectory->points.size() == 1) + { + // use steady-state gain + new_trajectory_gains->K_vec_.push_back(Ks); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(dt_)); + } + else + { + MatrixUX K; + MatrixXX Phi; + MatrixXU Gamma; + MatrixXX C; + C.setIdentity(); // we can measure every state + MatrixXX P(Ps), P_new; + VectorX x_p1; + VectorX x; + VectorU u{0}; + + // reserve space for feedback gains for every point in trajectory + // this is a design choice to save memory of the gains, theoretically we should + // save the gains at the dt-grid. + new_trajectory_gains->K_vec_.resize(trajectory->points.size() - 1); + new_trajectory_gains->time_vec_.resize(trajectory->points.size() - 1); + + double t_traj = get_time_from_duration(trajectory->points.back().time_from_start); + auto idx_traj = trajectory->points.size(); + + // iterate Riccati equation, backwards in time + while (t_traj > 0.0) + { + // TODO(christophfroehlich) interpolate from time or use trajectory class? + const trajectory_msgs::msg::JointTrajectoryPoint point = trajectory->points.at(idx_traj - 1); + x = get_state_from_point(point); + // system input acceleration, derivative of velocity + // we don't have acceleration in trajectory, so we have to calculate it + if (idx_traj < trajectory->points.size()) + { + double dt_traj = get_time_from_duration(trajectory->points.at(idx_traj).time_from_start) - + get_time_from_duration(point.time_from_start); + x_p1 = get_state_from_point(trajectory->points.at(idx_traj)); + u[0] = (x_p1(3) - x(3)) / dt_traj; + } + get_linear_system_matrices(x, u, dt_, Phi, Gamma); + riccati_step(K, P_new, P, Phi, Gamma, Q_, R_, N); + P = make_symmetric(P_new); + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production + // code + // DEBUG: wait to check if RT buffer works well + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + t_traj -= dt_; + if (t_traj < get_time_from_duration(trajectory->points.at(idx_traj - 1).time_from_start)) + { + idx_traj--; + if (idx_traj == 0lu) + { + // we reached the start of the trajectory + break; + } + // save feedback gain for this point + new_trajectory_gains->K_vec_.at(idx_traj - 1) = K; + new_trajectory_gains->time_vec_.at(idx_traj - 1) = + trajectory->points.at(idx_traj).time_from_start; + } + } + } + + trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); + + auto end_time = this->node_->now(); + auto duration = end_time - start_time; + RCLCPP_INFO( + node_->get_logger(), + "[CartpoleLqrTrajectoryPlugin] computed control law for %lu points in %es.", + trajectory->points.size(), duration.seconds()); + + new_trajectory_gains->print(); + + return true; +} + +bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( + const std::shared_ptr & trajectory) +{ + // parameters + auto N = MatrixUX::Zero(); + + MatrixUX Ks; + MatrixXX Ps; + auto x_R = get_state_from_point(trajectory->points.back()); + VectorU u_R{0}; + calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); + + auto new_trajectory_gains = std::make_shared(); + + // use steady-state gain + new_trajectory_gains->K_vec_.push_back(Ks); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(dt_)); + trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); + + new_trajectory_gains->print(); + + return true; +} + +bool CartpoleLqrTrajectoryPlugin::updateGainsRT() +{ + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + parseGains(); + } + + return true; +} + +void CartpoleLqrTrajectoryPlugin::parseGains() +{ + VectorX Q_diag{params_.gains.Q.data()}; + Q_ = MatrixXX{Q_diag.asDiagonal()}; + R_ = MatrixUU{params_.gains.R}; + // this is part of controller_interface definition, and not defined in GPL params_ + // if not set explicitly, it will be the cm_update rate + dt_ = 1. / node_->get_parameter("update_rate").as_int(); + + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] Q: " << std::endl << Q_); + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] R: " << std::endl << R_); + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] dt: " << std::endl << dt_); +} + +void CartpoleLqrTrajectoryPlugin::computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) +{ + if (!trajectory_active_lqr_ptr_->is_empty()) + { + auto e = get_state_from_point(error); + auto K = trajectory_active_lqr_ptr_->get_feedback_gain(duration_since_start); + // integrate acceleration from state feedback to velocity. consider sign of e! + u_ += period.seconds() * (K.dot(-e)); + // set system input as desired velocity + integrated LQR control output + tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u_; + } +} + +void CartpoleLqrTrajectoryPlugin::reset() +{ + trajectory_active_lqr_ptr_->reset(); + u_ = 0; +} + +void CartpoleLqrTrajectoryPlugin::start() +{ + // switch storage to new gains + trajectory_active_lqr_ptr_ = *trajectory_next_lqr_ptr_.readFromRT(); +} + +/* ----- private functions ----- */ + +void CartpoleLqrTrajectoryPlugin::calcLQR_steady( + const VectorX x, const VectorU u, const double dt, const MatrixXX Q, const MatrixUU R, + const MatrixUX N, MatrixUX & Ks, MatrixXX & Ps) const +{ + MatrixXX Phi; + MatrixXU Gamma; + MatrixXX C; + C.setIdentity(); + + get_linear_system_matrices(x, u, dt, Phi, Gamma); + + // solve steady-state Riccati equation iteratively + MatrixXX P, P_new; + MatrixUX K; + P = P.setIdentity() * 1e5; // initial value of P + + for (int ct = 0; ct < 1e4; ++ct) + { + riccati_step(K, P_new, P, Phi, Gamma, Q, R, N); + if ((P_new - P).norm() < 1e-3) + { // abort criterium + break; + } + P = make_symmetric(P_new); + } + Ks = K; + Ps = P; + + // std::cout << "Ks: " << Ks << std::endl; + // std::cout << "Ps: " << Ps << std::endl; +} + +void CartpoleLqrTrajectoryPlugin::riccati_step( + MatrixUX & K, MatrixXX & P_new, const MatrixXX & P, const MatrixXX & Phi, const MatrixXU & Gamma, + const MatrixXX & Q, const MatrixUU & R, const MatrixUX & N) const +{ + K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); + P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; +} + +void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( + const VectorX x, const VectorU u, const double dt, MatrixXX & Phi, MatrixXU & Gamma) const +{ + // states and input + double theta = x[0]; + double x_ddot = u[0]; + + // system parameters + double lS = params_.system.lS; + double mS = params_.system.mS; + double g = params_.system.g; + double IzzS = params_.system.IzzS; + + // Jacobians of continuous-time dynamics + MatrixXX J_x = MatrixXX::Zero(); + J_x(0, 1) = 1; + J_x(1, 0) = -0.2e1 * lS * mS * (g * cos(theta) - sin(theta) * x_ddot) / (lS * lS * mS + 4 * IzzS); + J_x(2, 3) = 1; + + MatrixXU J_u = MatrixXU::Zero(); + J_u(1, 0) = -0.2e1 * lS * mS * cos(theta) / (lS * lS * mS + 4 * IzzS); + J_u(3, 0) = 1; + + // discrete-time linearized system matrices + Phi = MatrixXX::Identity() + dt * J_x; + Gamma = dt * J_u; +} + +} // namespace ros2_control_demo_example_xx + +#include + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin, + joint_trajectory_controller_plugins::TrajectoryControllerBase) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml new file mode 100644 index 000000000..09b204c11 --- /dev/null +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -0,0 +1,45 @@ +ros2_control_demo_example_xx: + command_joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller plugin", + read_only: true, + validation: { + size_gt<>: [0], + } + } + system: + lS: { + type: double, + default_value: 1.0, + description: "Length of the pole" + } + mS: { + type: double, + default_value: 1.100, + description: "mass of the pole" + } + IzzS: { + type: double, + default_value: 0.092, + description: "moment of inertia of the pendulum about its center of mass" + } + g: { + type: double, + default_value: 9.80, + description: "gravitational constant" + } + gains: + R: { + type: double, + default_value: 1.0, + description: "Cost on the control input" + } + Q: { + type: double_array, + default_value: [1.0, 1.0, 1.0, 1.0], + description: "Cost on the state, diagonal of matrix", + validation: { + fixed_size<>: [4], + } + } diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp new file mode 100644 index 000000000..8e9ef809a --- /dev/null +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -0,0 +1,157 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ + +#include +#include +#include +#include + +#include "cartpole_lqr_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" + +// shortcut +namespace ros2_control_demo_example_xx +{ +// shortcut +#define NUM_STATES 4 +#define NUM_INPUTS 1 +typedef Eigen::Matrix MatrixUX; +typedef Eigen::Matrix MatrixXU; +typedef Eigen::Matrix MatrixXX; +typedef Eigen::Matrix MatrixUU; +typedef Eigen::Vector VectorX; +typedef Eigen::Vector VectorU; + +double get_time_from_duration(const builtin_interfaces::msg::Duration & time_from_start) +{ + return time_from_start.sec + time_from_start.nanosec * 1e-9; +}; + +// Make P symmetric +auto make_symmetric(MatrixXX & P) { return 0.5 * (P + P.transpose()); } + +class TrajectoryLQR +{ +public: + std::vector K_vec_; + std::vector time_vec_; + + /** + * @brief sample the feedback gain at the given time + */ + MatrixUX get_feedback_gain(const rclcpp::Duration & duration_since_start) const; + + /** + * @brief resets internal storage + */ + void reset() + { + K_vec_.clear(); + time_vec_.clear(); + }; + + /** + * @brief returns true if the trajectory is empty + */ + bool is_empty() const { return K_vec_.empty(); } + + /** + * @brief print the gains on std::cout + */ + void print() const + { + for (size_t i = 0; i < K_vec_.size(); ++i) + { + std::cout << "at t: " << get_time_from_duration(time_vec_.at(i)) << "s K: " << std::endl + << K_vec_.at(i) << std::endl; + } + } +}; + +class CartpoleLqrTrajectoryPlugin +: public joint_trajectory_controller_plugins::TrajectoryControllerBase +{ +public: + bool initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::vector map_cmd_to_joints) override; + + bool configure() override; + + bool activate() override; + + // computes the control law for a given trajectory, for non-RT thread + bool computeControlLawNonRT_impl( + const std::shared_ptr & trajectory) override; + + // fast version of computeControlLawNonRT_impl for a single point only, for RT thread + bool computeControlLawRT_impl( + const std::shared_ptr & trajectory) override; + + bool updateGainsRT() override; + + void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override; + + void reset() override; + + void start() override; + +protected: + /** + * @brief parse gains from parameter struct + */ + void parseGains(); + + void get_linear_system_matrices( + const VectorX x, const VectorU u, const double dt, MatrixXX & Phi, MatrixXU & Gamma) const; + void calcLQR_steady( + const VectorX q, const VectorU u, const double dt, const MatrixXX Q, const MatrixUU R, + const MatrixUX N, MatrixUX & Ks, MatrixXX & Ps) const; + void riccati_step( + MatrixUX & K, MatrixXX & P_new, const MatrixXX & P, const MatrixXX & Phi, + const MatrixXU & Gamma, const MatrixXX & Q, const MatrixUU & R, const MatrixUX & N) const; + + // integrator state for system input/LQR output + double u_; + + // number of command joints + size_t num_cmd_joints_; + // map from joints in the message to command joints + std::vector map_cmd_to_joints_; + + // Parameters from ROS for ros2_control_demo_example_xx + std::shared_ptr param_listener_; + Params params_; + // sampling time of the controller + double dt_ = 0.01; + // LQR cost function parameter for states + MatrixXX Q_; + // LQR cost function parameter for input + MatrixUU R_; + + // storage for LQR gains + std::shared_ptr trajectory_active_lqr_ptr_; + realtime_tools::RealtimeBuffer> trajectory_next_lqr_ptr_; +}; + +} // namespace ros2_control_demo_example_xx + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h b/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h new file mode 100644 index 000000000..3fdddedc9 --- /dev/null +++ b/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_XX_BUILDING_LIBRARY +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ diff --git a/example_XX/description/gazebo/cart_pole.gazebo.xacro b/example_XX/description/gazebo/cart_pole.gazebo.xacro new file mode 100644 index 000000000..e2b8e8edb --- /dev/null +++ b/example_XX/description/gazebo/cart_pole.gazebo.xacro @@ -0,0 +1,15 @@ + + + + + + + + + $(find ros2_control_demo_example_xx)/config/cart_pole_controllers.yaml + + +a + + + diff --git a/example_XX/description/launch/view_robot.launch.py b/example_XX/description/launch/view_robot.launch.py new file mode 100644 index 000000000..72a80732c --- /dev/null +++ b/example_XX/description/launch/view_robot.launch.py @@ -0,0 +1,98 @@ +# Copyright 2023 AIT - Austrian Institute of Technology +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="cart_pole.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_xx"), "urdf", description_file] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "cart_pole/rviz", "cart_pole.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro new file mode 100644 index 000000000..e5a70bba7 --- /dev/null +++ b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro @@ -0,0 +1,26 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + + + + + + + + + + + diff --git a/example_XX/description/urdf/cart_pole.urdf.xacro b/example_XX/description/urdf/cart_pole.urdf.xacro new file mode 100644 index 000000000..d05f291b3 --- /dev/null +++ b/example_XX/description/urdf/cart_pole.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_XX/doc/userdoc.rst b/example_XX/doc/userdoc.rst new file mode 100644 index 000000000..880c8cdf8 --- /dev/null +++ b/example_XX/doc/userdoc.rst @@ -0,0 +1,72 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_XX/doc/userdoc.rst + +.. _ros2_control_demos_example_xx_userdoc: + +Example xx: Cart-Pole +================================= + +With *example_xx*, we demonstrate the a robot with passive joints. + +.. note:: + + Follow the installation instructions on :ref:`ros2_control_demos_install` how to install all dependencies, + Gazebo Classic should be automatically installed. + + * If you have installed and compiled this repository locally, you can directly use the commands below. + * If you have installed it via the provided docker image: To run the first two steps of this example (without Gazebo Classic), use the commands as described with :ref:`ros2_control_demos_install`. To run the later steps using Gazebo Classic, execute + + .. code:: + + docker run -it --rm --name ros2_control_demos --net host ros2_control_demos ros2 launch ros2_control_demo_example_xx cart_pole.launch.py gui:=false + + first. Then on your local machine you can run the Gazebo Classic client with + + .. code-block:: shell + + gzclient + + and/or ``rviz2`` with + + .. code-block:: shell + + rviz2 -d src/ros2_control_demos/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz + + + For details on the ``gazebo_ros2_control`` plugin, see :ref:`gazebo_ros2_control`. + +Tutorial steps +-------------------------- + +The following example shows a cart with a pendulum arm. + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_xx cart_pole.launch.py + ros2 run ros2_control_demo_example_xx trajectory_action_client.py + + +Files used for this demos +------------------------- + +- Launch files: + + + View robot: `view_robot.launch.py `__ + + Gazebo Classic: `cart_pole.launch.py `__ + +- Controllers yaml: `cart_pole_controllers.yaml `__ +- URDF file: + + + Main File: `cart_pole.urdf.xacro `__ + + Description: `cart_pole_description.urdf.xacro `__ + + ``ros2_control`` tag: `cart_pole.ros2_control.xacro `__ + + ``gazebo`` tag: `cart_pole.gazebo.xacro `__ + +- RViz configuration: `cart_pole.rviz `__ +- Action client: `trajectory_action_client.py `__ + + +Controllers from this demo +-------------------------- +* ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ + +* ``Joint Trajectory Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_XX/package.xml b/example_XX/package.xml new file mode 100644 index 000000000..ae4ce6aaf --- /dev/null +++ b/example_XX/package.xml @@ -0,0 +1,47 @@ + + + + ros2_control_demo_example_xx + 0.0.0 + Demo package of `ros2_control` simulation with cart-pole. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Christoph Froehlich + + Apache-2.0 + + ament_cmake + ament_cmake_python + + backward_ros + pluginlib + rclcpp + rclpy + generate_parameter_library + joint_trajectory_controller_plugins + eigen + + python3-pandas + + controller_manager + gazebo_ros + gazebo_ros2_control + joint_state_broadcaster + joint_state_publisher_gui + joint_trajectory_controller + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/example_XX/plugins.xml b/example_XX/plugins.xml new file mode 100644 index 000000000..50432fd68 --- /dev/null +++ b/example_XX/plugins.xml @@ -0,0 +1,6 @@ + + + A trajectory controller consisting of a LQR for the cart pole example. + + diff --git a/example_XX/ros2_control_demo_example_xx/__init__.py b/example_XX/ros2_control_demo_example_xx/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/example_XX/scripts/trajectory_action_client.py b/example_XX/scripts/trajectory_action_client.py new file mode 100755 index 000000000..22e3a9761 --- /dev/null +++ b/example_XX/scripts/trajectory_action_client.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +# Copyright 2023 AIT - Austrian Institute of Technology +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pandas as pd +import rclpy +from trajectory_msgs.msg import JointTrajectoryPoint +from rclpy.node import Node +from builtin_interfaces.msg import Duration, Time +import sys +import signal +from rclpy.action import ActionClient +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance + + +class TrajectoryActionClient(Node): + def __init__(self): + super().__init__("trajectory_action_client") + self.idx = 0 + self.declare_parameter("file_name", "src/ros2_control_demos/example_XX/swingup") + self.declare_parameter("nrJoints", 2) + self.declare_parameter("joint_names", ["slider_to_cart", "cart_to_pendulum"]) + self.declare_parameter("path_tolerance", [100.1, 100.1]) + self.declare_parameter("goal_tolerance", [0.05, 0.05]) + self.declare_parameter("map_joints", [0, 1]) + + self.action_client_ = ActionClient( + self, FollowJointTrajectory, "/trajectory_controllers/follow_joint_trajectory" + ) + + # safe handling of SIGINT/SIGTERM + signal.signal(signal.SIGINT, self.safe_exit) + signal.signal(signal.SIGTERM, self.safe_exit) + + # ---------------------------------------- read Excel-file ------------ + + print("Load Excel-file") + fileName = self.get_parameter("file_name").get_parameter_value().string_value + fileExtension = ".csv" + fileNameWithPath = fileName + fileExtension + Data = pd.read_csv( + fileNameWithPath, + index_col=None, + header=0, + # usecols="A,B,C,D,E,F,G,H", + # , header=None + ) + self.Data_array = Data.values + print("File name = %s" % (fileNameWithPath)) # output file name with logging + + # ------------------------- store data from Excel-file in variables --- + self.nrJoints = self.get_parameter("nrJoints").value + self.map_joints = ( + self.get_parameter("map_joints").get_parameter_value().integer_array_value + ) + + self.Data_array_size = len(self.Data_array[0, :]) + self.trajLen = len(self.Data_array[:, 0]) + print("Length of trajectory: " + str(self.trajLen)) + + print("Data preview:") + print(Data.head(2)) + sys.stdout.flush() + + self.joint_names_ = self.get_parameter("joint_names").value + + self.send_goal() + + def send_goal(self): + self.is_goal_rejected = False + self.is_goal_accepted = False + + self.input_equals("\nStart the trajectory by pressing enter", "") + goal_msg = FollowJointTrajectory.Goal() + goal_msg.trajectory.joint_names = self.joint_names_ + goal_msg.trajectory.header.stamp = Time(sec=0, nanosec=0) # start now + + # get points from csv data + offset_idx = self.nrJoints + for i_traj in range(self.trajLen): + traj_point_message = JointTrajectoryPoint() + traj_point_message.positions = [0.0] * self.nrJoints + traj_point_message.velocities = [0.0] * self.nrJoints + duration_in_seconds = self.Data_array[i_traj, 0] + for i_pos in range(self.nrJoints): + traj_point_message.positions[i_pos] = self.Data_array[ + i_traj, 1 + self.map_joints[i_pos] + ] + traj_point_message.velocities[i_pos] = self.Data_array[ + i_traj, 1 + offset_idx + self.map_joints[i_pos] + ] + + traj_point_message.time_from_start = Duration( + sec=int(duration_in_seconds), + nanosec=int((duration_in_seconds - int(duration_in_seconds)) * 1e9), + ) + goal_msg.trajectory.points.append(traj_point_message) + + # set goal and path tolerances for new goal + # TODO: this does not work, see https://github.com/ros-controls/ros2_controllers/pull/716 + for i_pos in range(self.nrJoints): + goal_tolerance = JointTolerance() + goal_tolerance.name = self.joint_names_[i_pos] + goal_tolerance.position = self.get_parameter("goal_tolerance").value[i_pos] + goal_msg.goal_tolerance.append(goal_tolerance) + path_tolerance = JointTolerance() + path_tolerance.name = self.joint_names_[i_pos] + path_tolerance.position = self.get_parameter("path_tolerance").value[i_pos] + goal_msg.path_tolerance.append(path_tolerance) + + self.action_client_.wait_for_server() + self._send_goal_future = self.action_client_.send_goal_async( + goal_msg, self.get_feedback_callback + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + self.goal_handle = future.result() + if not self.goal_handle.accepted: + self.get_logger().info("Goal rejected!") + self.is_goal_rejected = True + return + + self.is_goal_accepted = True + self.get_logger().info("Goal accepted!") + + self._get_result_future = self.goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + print(result.error_code) + print(result.error_string) + if result.error_code == result.SUCCESSFUL: + print("Trajectory goal reached!") + self.send_goal() + + def get_feedback_callback(self, feedback): + + # print(feedback.feedback.desired) + # print(feedback.feedback.actual) + # print(feedback.feedback.error) + # print("Got feedback!") + feedback # eat warnings + + def safe_exit(self, *args): + # stop when the program is terminated from cmdline + if self.is_goal_accepted: + print("stop action") + future = self.goal_handle.cancel_goal_async() + future.add_done_callback(self.goal_canceled_callback) + + rclpy.shutdown() + + def goal_canceled_callback(self, future): + cancel_response = future.result() + if len(cancel_response.goals_canceling) > 0: + self.get_logger().info("Cancelling of goal complete") + else: + self.get_logger().warning("Goal failed to cancel") + + def input_equals(self, message, input_str): + text = "input" + while not text == input_str: + text = input(message) + + +def main(args=None): + rclpy.init(args=args) + + trajectory_action_client = TrajectoryActionClient() + + try: + rclpy.spin(trajectory_action_client) + finally: + trajectory_action_client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/example_XX/sidestep_low_left.csv b/example_XX/sidestep_low_left.csv new file mode 100644 index 000000000..8d8331a62 --- /dev/null +++ b/example_XX/sidestep_low_left.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,1.0,0.0,0.0,0.0,-13.582842304726647 +0.1,0.9908400049720579,0.01325045947812364,-0.1831999005715079,0.26500918950732566,-24.22806648751422 +0.2,0.9623566453157602,0.052565166873090374,-0.38646729255444856,0.52128495839201,-18.259059275845477 +0.30000000000000004,0.9166427183024465,0.10931412918041443,-0.5278112477118159,0.6136942877544568,-12.409036061429301 +0.4,0.8597160223395942,0.16673016926182568,-0.6107226715452487,0.5346265138737955,-7.2450579701905 +0.5,0.7969366091968367,0.20897819835507048,-0.6448655913099005,0.3103340679911038,-3.168999707927978 +0.6000000000000001,0.7325144382934369,0.22411292945016167,-0.6435778267581107,-0.007639446089287271,-0.35800513557509617 +0.7000000000000001,0.6692669802334312,0.2060214565712224,-0.6213713344420202,-0.3541900114895065,1.1702208961271794 +0.8,0.6085709733682355,0.15526386244161725,-0.5925488028619008,-0.6609618711025866,1.496266501741761 +0.9,0.550440893312543,0.0789076728029028,-0.5700527982519421,-0.8661619216717187,0.8424467884987269 +1.0,0.49377839635442694,-0.010725728068270426,-0.5631971409103624,-0.9265060957516191,-0.3493165885348479 +1.1,0.4368766314951924,-0.09849539661859454,-0.5748381562743512,-0.8288872752548936,-1.4650534397548274 +1.2000000000000002,0.37812064688200187,-0.1696677891950051,-0.6002815359894682,-0.5945605762733314,-1.916160952165578 +1.3,0.3166562921794397,-0.21302135655203985,-0.6290055580617843,-0.27251077086738457,-1.3121255256793616 +1.4000000000000001,0.252828781822614,-0.22300268299049306,-0.6475446490747488,0.07288424209831199,0.532195156617898 +1.5,0.1883792282949881,-0.20069615558897821,-0.6414464214777829,0.3732463059319788,3.6797330199316685 +1.6,0.12649024436409825,-0.15374203695777106,-0.5963332571400103,0.5658360666921706,8.087921167936358 +1.7000000000000002,0.07169626725907002,-0.09523451300126391,-0.49954628496054043,0.6043144124380037,13.532466373327605 +1.8,0.02957881217220647,-0.04150615853439743,-0.3428028167767392,0.4702526768993162,19.563572248350802 +1.9000000000000001,0.006219335666416475,-0.008996762346338082,-0.12438671333905998,0.1799352468618647,25.597486939648455 +2.0,0.0,0.0,0.0,0.0, diff --git a/example_XX/sidestep_low_right.csv b/example_XX/sidestep_low_right.csv new file mode 100644 index 000000000..4eeea1b14 --- /dev/null +++ b/example_XX/sidestep_low_right.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,0.0,0.0,0.0,13.582834681774651 +0.1,0.009159998805574011,-0.013250464940438968,0.1831999761137826,-0.2650092987687595,24.22808970251364 +0.2,0.037643367135360706,-0.052565184108354666,0.3864673904819508,-0.5212850845895562,18.259040901977798 +0.30000000000000004,0.0833573037937571,-0.10931415711133492,0.5278113426859683,-0.6136943754700693,12.409054285177184 +0.4,0.14028401155960885,-0.1667302070099039,0.610722812631055,-0.5346266225012926,7.245049840839648 +0.5,0.20306343560767343,-0.20897823937905882,0.6448656683302373,-0.31033402488180983,3.168995317107675 +0.6000000000000001,0.26748560733101473,-0.2241129537452494,0.6435777661366053,0.0076397375579876315,0.35798161654959254 +0.7000000000000001,0.33073305334271935,-0.20602144259604266,0.6213711540975038,0.3541904854261542,-1.1702220386857307 +0.8,0.3914290444317056,-0.15526380693065628,0.5925486676822251,0.6609622278815681,-1.4962566182800145 +0.9,0.44955911646814123,-0.0789075946714122,0.5700527730464776,0.866162017303333,-0.8424351388021747 +1.0,0.5062216168861734,0.010725801254260484,0.56319723531419,0.9265059012101524,0.34932827353212964 +1.1,0.5631233967221619,0.0984954378668856,0.5748383614055668,0.8288868310423773,1.4650636813635405 +1.2000000000000002,0.6218793975421866,0.16966779078740932,0.600281654994933,0.5945602273681059,1.9161327770081475 +1.3,0.6833437534882811,0.2130213395128202,0.6290054639269657,0.2725107471400949,1.3121099393996063 +1.4000000000000001,0.7471712506067291,0.22300267025047876,0.6475444784420132,-0.0728841323869364,-0.5321952059388131 +1.5,0.8116207944340279,0.20069614387838913,0.6414463981039753,-0.37324639505485,-3.6797024941986103 +1.6,0.8735097810616919,0.15374201036630308,0.5963333344492998,-0.5658362751868663,-8.087930632335542 +1.7000000000000002,0.9283037569698974,0.09523448102646237,0.49954618371479753,-0.6043143116099807,-13.532493179839031 +1.8,0.9704211992933057,0.041506142345594264,0.34280266275336,-0.4702524620073983,-19.563555969370874 +1.9000000000000001,0.993780666216168,0.008996759623795886,0.12438667570388585,-0.1799351924285638,-25.597479194421577 +2.0,1.0,0.0,0.0,0.0, diff --git a/example_XX/sidestep_up_left.csv b/example_XX/sidestep_up_left.csv new file mode 100644 index 000000000..99a328a2e --- /dev/null +++ b/example_XX/sidestep_up_left.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,1.0,3.141592653589793,0.0,0.0,25.727222325497788 +0.1,1.0131967205717793,3.1621392896044482,0.26393441147946906,0.41093272034183115,28.427563725305465 +0.2,1.0446805712312321,3.214300376886786,0.365742601709609,0.6322890253049573,-8.053814755431427 +0.30000000000000004,1.0727725120916252,3.272368210966063,0.19609621549825854,0.5290676562805826,-28.277225491338164 +0.4,1.0770437032803744,3.313641489926836,-0.11067239172328348,0.296397922934826,-37.2429050694341 +0.5,1.0483117721579391,3.3304205767736565,-0.4639662307254031,0.039183814001593376,-38.56887738441252 +0.6000000000000001,0.9848857328264592,3.322766846328476,-0.8045545559042157,-0.19225842290519513,-34.91622016919236 +0.7000000000000001,0.889904067170022,3.294287399420048,-1.095078757224509,-0.3773305152633704,-28.127853041475003 +0.8,0.7695175554342428,3.2499629988004664,-1.3126514774910374,-0.5091574971282213,-19.431738056537547 +0.9,0.6316923907716203,3.195201167299268,-1.4438518157612081,-0.5860791328957171,-9.653766676708562 +1.0,0.4854110980005052,3.1355008802567093,-1.4817740396609596,-0.6079266079554391,0.6061625205688302 +1.1,0.34009387893961884,3.076368133612908,-1.4245703415568365,-0.5747283249205931,10.843605473722643 +1.2000000000000002,0.20511158058826864,3.0233087198590716,-1.2750756254699807,-0.48645995015617455,20.54133620246714 +1.3,0.08928842899531193,2.9818131438692546,-1.041387406389295,-0.3434515696402237,29.060110809727608 +1.4000000000000001,0.0002895324189204924,2.9572385490827298,-0.7385905251385729,-0.14804032609025766,35.49723335244904 +1.5,-0.05624415925942309,2.9543834874427968,-0.3920833084282454,0.0909390932916079,38.501628227761735 +1.6,-0.07800178917435524,2.9763453328069662,-0.04306928987045941,0.34829781399180226,36.05018372967955 +1.7000000000000002,-0.06797701410719119,3.021995398100074,0.2435647912137226,0.5647034918703048,25.256542703251032 +1.8,-0.03733141300472916,3.0812263540516276,0.36934723083551474,0.6199156271607722,2.2162626076042717 +1.9000000000000001,-0.00943202573156447,3.126907394498823,0.18864051462780426,0.2937051817831894,-38.59189016444331 +2.0,0.0,3.141592653589793,0.0,0.0, diff --git a/example_XX/sidestep_up_right.csv b/example_XX/sidestep_up_right.csv new file mode 100644 index 000000000..5d1141234 --- /dev/null +++ b/example_XX/sidestep_up_right.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,3.141592653589793,0.0,0.0,-25.726955400977594 +0.1,-0.01319663724708581,3.121046147293422,-0.2639327449088351,-0.41093012600776374,-28.427488665402784 +0.2,-0.044680372107853535,3.0688852603510104,-0.3657419523065087,-0.632287612840499,8.053527713728787 +0.30000000000000004,-0.07277236862408736,3.010817411663038,-0.196097978018168,-0.529069360918943,28.277010882465976 +0.4,-0.0770438392399718,2.96954382136828,0.11066856570048476,-0.2964024449761723,37.242690145633944 +0.5,-0.04831225726168709,2.9527643154940555,0.46396307386521624,-0.03918767250831095,38.56923147155754 +0.6000000000000001,0.015113614853642935,2.9604178390552467,0.8045543684413661,0.19225814373213537,34.91648579551417 +0.7000000000000001,0.11009537913580322,2.9888973880774365,1.0950809172018487,0.3773328367116665,28.128080947670394 +0.8,0.23048215206842299,3.033222058802621,1.3126545414504873,0.5091605777920026,19.43170385221482 +0.9,0.3683075879105022,3.087984136246791,1.4438541753910483,0.5860809710914082,9.653659501849429 +1.0,0.5145890612883659,3.147684527713766,1.4817752921661829,0.6079268582481018,-0.6062826320262397 +1.1,0.6599064111804592,3.20681731664309,1.424571705675741,0.5747289203383725,-10.843463499966113 +1.2000000000000002,0.7948888929565243,3.2598768763037,1.2750779298456454,0.48646227287383714,-20.541287057644332 +1.3,0.9107122693287941,3.3013727077557307,1.0413895975997745,0.3434543561667906,-29.060188518636096 +1.4000000000000001,0.9997112471290509,3.325947424022019,0.738589958405365,0.14803996915898412,-35.497733451904544 +1.5,1.056244695189266,3.328802215276431,0.392079002798928,-0.0909441440707547,-38.50190903689841 +1.6,1.0780018809576026,3.3068398558692222,0.04306471256781089,-0.34830304407344753,-36.0499636811846 +1.7000000000000002,1.0679767835716063,3.2611894467772236,-0.2435666602877448,-0.5647051377665463,-25.256202053248504 +1.8,1.0373311671436036,3.2019585502833987,-0.36934566827230775,-0.6199127921099595,-2.2158907192831525 +1.9000000000000001,1.0094319418642066,3.1562777821354597,-0.18863883731563338,-0.2937025708487912,38.59154699727519 +2.0,1.0,3.141592653589793,0.0,0.0, diff --git a/example_XX/swingdown.csv b/example_XX/swingdown.csv new file mode 100644 index 000000000..6b5f26200 --- /dev/null +++ b/example_XX/swingdown.csv @@ -0,0 +1,82 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,3.141592653589793,0.0,0.0,0.8980980968135133 +0.1,0.0004896649596369584,3.142355123974358,0.009793298292100449,0.0152494077253126,1.1112087627417346 +0.2,0.0017406019392048767,3.144419461859018,0.015225441299257883,0.026037349967879896,-0.016433486596925884 +0.30000000000000004,0.00309302502961027,3.1470736357868807,0.011823020508849955,0.027046128589377036,-0.7418827832236262 +0.4,0.0038288989777238436,3.1496050234652375,0.002894458453421439,0.0235816249777562,-1.1977295735281952 +0.5,0.0035044866082561293,3.151709377795235,-0.009382705842775728,0.018505461622190713,-1.4739075795762786 +0.6000000000000001,0.0018563489883896482,3.1532979761192617,-0.023580046554553886,0.013266504858342142,-1.6301627551386686 +0.7000000000000001,-0.0012605088396836816,3.1543877424717164,-0.03875711000691232,0.00852882219075312,-1.705734216468739 +0.8,-0.005913047380817745,3.1550409027900645,-0.05429366081576932,0.004534384176206157,-1.727359578534122 +0.9,-0.012116989137624275,3.1553327620981415,-0.06978517432036145,0.0013028019853370582,-1.7134357779474039 +1.0,-0.019854659484664437,3.1553354279300496,-0.08496823262044174,-0.0012494853471709457,-1.6755566520404839 +1.1,-0.029086446707295543,3.155110982634697,-0.09966751183217953,-0.0032394205598901492,-1.6211867168887624 +1.2000000000000002,-0.039758298488405314,3.154709296374018,-0.11376952379001466,-0.004794304653672454,-1.5563886020403435 +1.3,-0.05180674396814163,3.1541678273683758,-0.12719938580471063,-0.006035075459183664,-1.4836836730853995 +1.4000000000000001,-0.06516195688578291,3.153512575893647,-0.1399048725481111,-0.007069954035391924,-1.4056822752759301 +1.5,-0.07974987995218227,3.1527587493147924,-0.15185358877986915,-0.008006577541703256,-1.3241573747967887 +1.6,-0.09549360413019388,3.151910938231885,-0.16302089478036874,-0.008949644116443715,-1.239072919871877 +1.7000000000000002,-0.11231418190166902,3.1509625518837137,-0.17339066064914113,-0.010018082846986095,-1.1521853376524365 +1.8,-0.13013126455848215,3.1498938193565764,-0.18295099248708876,-0.011356567695756135,-1.0623869547505147 +1.9000000000000001,-0.14886324188853667,3.148668552138284,-0.1916885541140103,-0.013148776670092305,-0.970028601866152 +2.0,-0.16842712621290823,3.1472287968245434,-0.19958913237338086,-0.01564632960471758,-0.8738091043550451 +2.1,-0.18873822791484507,3.145486487855202,-0.20663290166536805,-0.0191998497821099,-0.7726844481549661 +2.2,-0.20970954461530003,3.1433109465648155,-0.21279343234374345,-0.02431097602562016,-0.6645577494530157 +2.3000000000000003,-0.23125067378786315,3.140510613529957,-0.2180291511075292,-0.03169568467153932,-0.5457869124900124 +2.4000000000000004,-0.2532658008723289,3.1368069222597876,-0.22227339058179277,-0.04237814073186093,-0.4110241453814053 +2.5,-0.27565101842136386,3.131795928340245,-0.22543096039889687,-0.05784173765898385,-0.2541964711091684 +2.6,-0.29829046041415674,3.1248926445432876,-0.2273578794569579,-0.08022393828016433,-0.06331427179159357 +2.7,-0.32104942639483347,3.1152524462905986,-0.22782144015656883,-0.11258002677362543,0.17943100986374397 +2.8000000000000003,-0.3437639617660014,3.1016584475636426,-0.22646926726682917,-0.15929994776550016,0.49732748174430363 +2.9000000000000004,-0.3662255822480557,3.0823599064696037,-0.22276314237429423,-0.2266708741152824,0.9285033360059032 +3.0,-0.3881572255335267,3.054843720074165,-0.2158697233351155,-0.32365285379348985,1.5272494632304612 +3.1,-0.40917611061048137,3.015512704251639,-0.20450797820396557,-0.46296746265702476,2.3720547468996873 +3.2,-0.42873766724857854,2.9592360199383907,-0.18672315455796396,-0.66256622360793,3.567184971676484 +3.3000000000000003,-0.44605179163447584,2.8787321812440676,-0.15955933316004306,-0.9475105502785446,5.236895895390032 +3.4000000000000004,-0.459962036054507,2.7637491312010822,-0.11864555524056082,-1.3521504505811954,7.466954481647186 +3.5,-0.4687889865580481,2.600040655057765,-0.057893454830299465,-1.9220190722848844,10.109641625627033 +3.6,-0.4701862704605304,2.3682818061164386,0.029947776780646175,-2.7131579065416784,12.252034157871782 +3.7,-0.4612120095236938,2.043615687579791,0.14953744195611735,-3.7801644641908188,11.182905333347946 +3.8000000000000003,-0.43929357192739477,1.5985507117653028,0.2888313099698903,-5.12113505209843,1.728062852065051 +3.9000000000000004,-0.4059929982589307,1.0178437316877835,0.37718016339927235,-6.493004549451259,-19.53626204512096 +4.0,-0.3744608756518423,0.3344651197287577,0.25346228874239496,-7.174567689728002,-40.89726206649438 +4.1000000000000005,-0.36817218951644054,-0.3553951846456218,-0.1276885660343651,-6.62263839776081,-38.891673658665475 +4.2,-0.40116226775695646,-0.9508720544211346,-0.5321129987759737,-5.286898997752897,-19.316043092299775 +4.3,-0.46531120892520844,-1.4055192694285705,-0.7508658245892018,-3.8060453023955882,-3.182993515840792 +4.4,-0.5427499072240677,-1.712669777288166,-0.797908141387946,-2.3369648547988207,3.3096980647061787 +4.5,-0.6209993037443283,-1.8742489685072985,-0.7670797890172317,-0.8946189695850393,2.981333342191629 +4.6000000000000005,-0.69595053292025,-1.8929123978038993,-0.7319447945011752,0.5213503836530551,0.1391127194208197 +4.7,-0.7688851595897345,-1.769642186561791,-0.7267477388884998,1.9440538411889423,-1.2561271326200802 +4.800000000000001,-0.8427156659797289,-1.50249728866807,-0.7498623889113916,3.398844116680922,3.0478002124550216 +4.9,-0.9180013713586771,-1.0935134576946153,-0.7558517186676181,4.7808325027843965,16.494180625702253 +5.0,-0.9878784538699831,-0.5698255168418146,-0.6416899315586427,5.692926314272013,34.94033493317614 +5.1000000000000005,-1.036433511867334,-0.0027744421832299297,-0.32941122838858855,5.648095178904186,42.80431219739947 +5.2,-1.0487893583739194,0.5151194333624814,0.08229429825694297,4.709782332019062,33.13323732383973 +5.300000000000001,-1.0241223445274774,0.9197483202017762,0.41104597867194353,3.382795404764735,18.310527048229503 +5.4,-0.9738115514595095,1.1873081193311825,0.5951698826872703,1.9684005778213873,8.27951170062056 +5.5,-0.9101005632475544,1.3123447736587521,0.6790498815517286,0.5323325087280888,3.627011420900077 +5.6000000000000005,-0.8402534619489789,1.2938708562888341,0.7178921444201042,-0.9018108561259277,0.6623630866536132 +5.7,-0.7676822472094255,1.134624119559335,0.7335321503709314,-2.283123878465559,-5.033332590621164 +5.800000000000001,-0.6958992617757562,0.8470056889708181,0.7021275583024962,-3.469244733306682,-15.944791400573198 +5.9,-0.6324694783638981,0.4645532119784668,0.5664681099347891,-4.179804806540196,-28.723101707152214 +6.0,-0.5890805367592223,0.0474081389755057,0.3013107221589057,-4.163096653523947,-34.232625603103436 +6.1000000000000005,-0.5750581791966085,-0.3350662341218295,-0.020863570906553962,-3.48639080842075,-28.304151106089527 +6.2,-0.5904512405617786,-0.6321303247279811,-0.28699765639688407,-2.4548910037000153,-16.851520094457275 +6.300000000000001,-0.6270208963641315,-0.8199410647664278,-0.44439545965004823,-1.3013237970662628,-6.24091495273785 +6.4,-0.6743618031219135,-0.8916209889802971,-0.5024226755057242,-0.13227468721162922,2.1919224864127247 +6.5,-0.7235714649504932,-0.8496416997712712,-0.48177056106594895,0.9718604713928619,10.309687597992536 +6.6000000000000005,-0.7668619338222116,-0.7063186803631503,-0.3840388163684142,1.8945999167681746,19.94029826420947 +6.7,-0.7957491297917574,-0.4877168951765774,-0.1937051030225677,2.4774357869632575,30.13028948482256 +6.800000000000001,-0.8008354561635319,-0.23422919292778654,0.0919785755871028,2.5923182580116046,36.748468953948255 +6.9,-0.7745846357679811,0.008006638866464105,0.43303783232395227,2.252398377875861,36.24163682942152 +7.0,-0.7149900207306187,0.20138758875389037,0.7588544684232612,1.61522061987069,29.2106086051237 +7.1000000000000005,-0.626557021699009,0.3255922215871434,1.0098055122089014,0.8688720367926253,18.70223340190598 +7.2,-0.518243100543454,0.3768171945732728,1.1564729109022718,0.15562742293034776,7.001344992384389 +7.300000000000001,-0.4009245089892408,0.36344195757848335,1.1898989201821193,-0.4231321628260471,-4.84948894416588 +7.4,-0.2858597501443424,0.3026891723922275,1.1113962567160176,-0.7919235408990056,-15.844350099666507 +7.5,-0.18356357612394775,0.2174388101297234,0.934527223692065,-0.9130837043511333,-24.2800202025846 +7.6000000000000005,-0.10228970355598846,0.13136249285357085,0.6909502276673065,-0.8084426411720554,-28.311773261349494 +7.7,-0.04632658313154299,0.06297569771286457,0.4283121808213341,-0.5592932616422452,-26.988222586113274 +7.800000000000001,-0.014946660411973017,0.021109318186221818,0.19928627356987313,-0.278034328890364,-20.588667670906553 +7.9,-0.002491173368576613,0.003603800877532851,0.04982346729808403,-0.07207601728338339,-10.253041957485154 +8.0,0.0,0.0,0.0,0.0, diff --git a/example_XX/swingup.csv b/example_XX/swingup.csv new file mode 100644 index 000000000..25385448e --- /dev/null +++ b/example_XX/swingup.csv @@ -0,0 +1,82 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,0.0,0.0,0.0,-4.403090628950663 +0.1,-0.004539967043690484,0.006567578479378633,-0.09079934055534572,0.13135157053477234,-14.336821359062391 +0.2,-0.02273162951555217,0.03194798245423959,-0.2730339088818896,0.37625650896244817,-23.537839246957386 +0.30000000000000004,-0.06237033141829319,0.08378203483900226,-0.5197401291729447,0.6604245387327948,-28.37164839967838 +0.4,-0.12743339135962653,0.16034667293525184,-0.7815210696537127,0.8708682231921904,-27.921389547369944 +0.5,-0.21681180430277192,0.2489875275102259,-1.006047189209185,0.9019488683072834,-22.290934617044996 +0.6000000000000001,-0.32463740293892646,0.3286600279313409,-1.150464783513884,0.6915011401150791,-12.81309376357328 +0.7000000000000001,-0.44165775305843236,0.3752673021198155,-1.189942218876299,0.24064434365443615,-1.3938443679687373 +0.8,-0.5569596689653602,0.36746852409796266,-1.1160960992622515,-0.3966199040914958,10.519948439174561 +0.9,-0.6593092560098988,0.2908594034147643,-0.9308956416285196,-1.1355625095724688,22.05596982534712 +1.0,-0.7382951197342537,0.14093191712530337,-0.6488216328585825,-1.8629872162166896,31.831857636290803 +1.1,-0.7862236622468436,-0.07311825992593296,-0.309749217393208,-2.4180163248080597,37.059867232838386 +1.2000000000000002,-0.8007770109134821,-0.32450520027195306,0.01868224406043229,-2.609722482112431,35.08043406690576 +1.3,-0.7862253565125626,-0.5710823532465932,0.2723508439579582,-2.3218205773801213,26.804813555458352 +1.4000000000000001,-0.7512191507287564,-0.7671684546002095,0.42777327171815815,-1.5999014496921895,16.564881403350938 +1.5,-0.7049963409616377,-0.8769743892029062,0.49668292362422084,-0.5962172423618473,7.598605523796512 +1.6,-0.6556233670012954,-0.8796930078411156,0.49077655558262123,0.541844869597657,-0.34691255993324266 +1.7000000000000002,-0.6110697874219091,-0.766887078972639,0.40029503600510635,1.7142737077718935,-9.474243507408476 +1.8,-0.5808844940097705,-0.5390974608346483,0.20341083223766757,2.841518654987698,-20.899475261237065 +1.9000000000000001,-0.5753514043865737,-0.20789092143287294,-0.09274903977373222,3.78261213304796,-31.49901237035887 +2.0,-0.600393699792452,0.1940395326584534,-0.40809686834383957,4.2559969487788205,-33.85784628755254 +2.1,-0.6523154482210297,0.6070148792619021,-0.6303381002276928,4.003509983289554,-24.891126073211257 +2.2,-0.7199571003574637,0.9615792677054635,-0.7224949425009457,3.087777785582106,-11.962333014509005 +2.3000000000000003,-0.7926659739059346,1.2063410746721273,-0.7316825284685285,1.807458353751302,-2.910554830768037 +2.4000000000000004,-0.8646270105389016,1.316601749759348,-0.707538204190815,0.3977551479931468,1.377986372466846 +2.5,-0.9328767906047193,1.2844667772652634,-0.6574573971255426,-1.040454597874811,4.451562937509129 +2.6,-0.9930192560378676,1.1088720510421795,-0.5453919115374358,-2.4714399265864584,10.765150128486487 +2.7,-1.0359009909507688,0.791850259305105,-0.31224278672059025,-3.8689959081551586,23.079176368164696 +2.8000000000000003,-1.048458213906512,0.34281562960372813,0.061098327605725374,-5.1116966858725394,37.970113577020015 +2.9000000000000004,-1.0224317658636146,-0.2019830983533615,0.4594306332522275,-5.784277873269352,42.46034674057756 +3.0,-0.9643427874037644,-0.7642027011895978,0.7023489359447768,-5.4601141834552855,29.098083414702003 +3.1,-0.8912252953923374,-1.253111273198937,0.760000904283799,-4.318057256731427,11.136459667165761 +3.2,-0.816272142441897,-1.6131403870313399,0.7390621547250231,-2.8825250199165176,1.04116771201548 +3.3000000000000003,-0.7431456690807253,-1.8291324832819604,0.7234673124984259,-1.4373169050963417,-0.7404236295181752 +3.4000000000000004,-0.6699266705120884,-1.9020904820658,0.7409126588742506,-0.02184307058019866,1.6328964816747367 +3.5,-0.5938588318163679,-1.8331093227664423,0.7804441150400883,1.4014662565672134,4.072150416815463 +3.6,-0.5151346195186446,-1.6203044782806464,0.7940401309144063,2.8546306331488247,2.487650167355039 +3.7,-0.4406292790635794,-1.2610484869672982,0.6960666781869317,4.330489193118279,-7.403687168080474 +3.8000000000000003,-0.38566190487363916,-0.7546242484715866,0.4032808056119048,5.797995576796658,-26.472778776856114 +3.9000000000000004,-0.36687719952345366,-0.11726284410665795,-0.027586698608200285,6.949232510501926,-42.83021953133303 +4.0,-0.3848814472739218,0.5834904701397841,-0.3324982564011413,7.065833774426743,-35.176640975036676 +4.1000000000000005,-0.419611136232232,1.238697576092158,-0.3620955227650963,6.038308344621699,-11.101164209020258 +4.2,-0.4497700609787529,1.7716489703706968,-0.24108297216532457,4.620719540946969,6.276339787711619 +4.3,-0.4670725782997097,2.1710748876333694,-0.10496737425381406,3.367798804306375,12.159214419255632 +4.4,-0.47214735080000275,2.4596369217925766,0.003471924247955181,2.4034418788786116,11.680325808588183 +4.5,-0.4679242108394951,2.664690304547718,0.08099087496219745,1.6976257762241465,9.215871244426518 +4.6000000000000005,-0.4571747855498045,2.8091873241307956,0.13399763083160815,1.1923146154373576,6.702795523989609 +4.7,-0.4419991164099382,2.910539734380356,0.16951575196572025,0.8347335895538702,4.694235416135336 +4.800000000000001,-0.42387241659706953,2.981449265175817,0.19301824429165348,0.5834570263553359,3.2207407178313825 +4.9,-0.4038049741105546,3.03100656018601,0.20833060543864731,0.4076888738485182,2.173088426570403 +5.0,-0.38248778512486953,3.0656476690881758,0.2180131742750621,0.285133304194785,1.4325250543101793 +5.1000000000000005,-0.360399678477496,3.089898618380898,0.22374895867240974,0.19988568165965356,0.9044350457382715 +5.2,-0.33788038815261767,3.106928013625275,0.22663684782514898,0.14070222322788958,0.5206236241384486 +5.300000000000001,-0.31517892609246145,3.118947098524287,0.22739239337798184,0.0996794747523499,0.23385787443045758 +5.4,-0.2924852580107236,3.127495245508811,0.22648096825677555,0.07128346493813084,0.012100692547606477 +5.5,-0.26995098429246867,3.1336418881862222,0.22420450610831566,0.051649388610092335,-0.16636135038498537 +5.6000000000000005,-0.24770279800679662,3.1381285827675836,0.2207592196051269,0.03808450301713759,-0.3158069781958893 +5.7,-0.2258512127040312,3.1414685771137782,0.2162724864501604,0.028715383906750436,-0.44608114589606834 +5.800000000000001,-0.20449622810607834,3.144016349630042,0.2108272055088984,0.02224006641852563,-0.5634123058911273 +5.9,-0.18373098963712964,3.1460160453467125,0.20447756387007812,0.017753847914881892,-0.6722671608074265 +6.0,-0.16364415286078593,3.1476350836179177,0.19725917165679793,0.014626917509220615,-0.7752976341281789 +6.1000000000000005,-0.14432138425620708,3.14898739355341,0.18919620043478094,0.01241928120062738,-0.8744605630321567 +6.2,-0.12584629541118617,3.150149361918534,0.180305576465633,0.010820086101863566,-0.9708156509497355 +6.300000000000001,-0.10830097350094355,3.151170675302601,0.1706008617392238,0.009606181579471195,-1.0650056432737174 +6.4,-0.09176618435398666,3.1520815936632856,0.1600949211999151,0.008612185634218807,-1.1571700999515204 +6.5,-0.07632135957162563,3.1528975926242766,0.14880157444731051,0.007707793585604551,-1.2473354113632387 +6.6000000000000005,-0.062044398054685014,3.153622036316146,0.13673765589150372,0.006781080251780027,-1.3349989148566008 +6.7,-0.049011139409104,3.1542475108985264,0.12392751702012586,0.005728411395822755,-1.4190733589248965 +6.800000000000001,-0.03729438788705122,3.1547562113215895,0.1104075134209294,0.00444559706543621,-1.4979567832024028 +6.9,-0.02696241744590261,3.1551195484072423,0.09623189540204458,0.00282114464763199,-1.5691710122418345 +7.0,-0.01807668856791361,3.155297335549895,0.08148268215773521,0.0007345982054157112,-1.6288900116684375 +7.1000000000000005,-0.010688303024193473,3.155237168512657,0.06628502871666732,-0.00193793895018388,-1.671263545154879 +7.2,-0.00483261232052018,3.1548748133920435,0.05082878535679939,-0.00530916346208096,-1.6878079841940998 +7.300000000000001,-0.0005210057875410092,3.1541371789997927,0.03540334530278396,-0.009443524382931733,-1.6652950518089904 +7.4,0.0022716632927734233,3.1529507289202603,0.02045003630350484,-0.014285477207716259,-1.5843239485610294 +7.5,0.003626192846594814,3.15126001307949,0.006640554772922894,-0.01952883960769178,-1.4155766846390554 +7.6000000000000005,0.0037080572787918076,3.1490643970503758,-0.005003266128983026,-0.02438348097459484,-1.115456884614229 +7.7,0.002811628969080266,3.146486831633828,-0.012925300065247817,-0.02716782735635895,-0.6179817299092234 +7.800000000000001,0.0014275117593718896,3.143897490642365,-0.014757044128919735,-0.02461899247289809,0.1749954296373198 +7.9,0.0003448297747109859,3.142129597304204,-0.006896595564298339,-0.010738874290320376,1.4108476709784925 +8.0,0.0,3.141592653589793,0.0,0.0, diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index b7aedafbf..799601324 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -13,6 +13,11 @@ install( DESTINATION share/${PROJECT_NAME}/diffbot ) +install( + DIRECTORY cart_pole/urdf cart_pole/rviz + DESTINATION share/${PROJECT_NAME}/cart_pole +) + install( DIRECTORY r6bot/meshes r6bot/srdf r6bot/urdf r6bot/rviz DESTINATION share/${PROJECT_NAME}/r6bot diff --git a/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz b/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz new file mode 100644 index 000000000..4380ea3d8 --- /dev/null +++ b/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz @@ -0,0 +1,176 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + cart: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pendulum: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slideBar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.673279762268066 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3353983461856842 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.060397744178772 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 846 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b0000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 495 + Y: 82 diff --git a/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro new file mode 100644 index 000000000..572634be4 --- /dev/null +++ b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +