├── towr ├── .gitignore ├── doc │ ├── nodes.jpg │ ├── phase_nodes.png │ ├── phase_nodes2.png │ ├── rosdoc.yaml │ ├── towr_motion_snaps.png │ ├── towr_problem_with_code.png │ ├── phase_based_parameterization.png │ └── mainpage.md ├── matlab │ ├── com_motion_derivatives.m │ ├── gap_height_map.m │ ├── euler_converter.m │ ├── cubic_hermite_polynomial.m │ └── plot_rosbag.m ├── package.xml ├── cmake │ └── towr-config.cmake ├── test │ ├── dynamic_constraint_test.cc │ └── dynamic_model_test.cc ├── src │ ├── nodes_observer.cc │ ├── phase_durations_observer.cc │ ├── state.cc │ ├── dynamic_model.cc │ ├── nodes_variables_all.cc │ ├── soft_constraint.cc │ ├── linear_constraint.cc │ ├── node_cost.cc │ ├── total_duration_constraint.cc │ ├── robot_model.cc │ ├── spline_acc_constraint.cc │ ├── spline_holder.cc │ ├── time_discretization_constraint.cc │ ├── monoped_gait_generator.cc │ ├── spline.cc │ ├── base_motion_constraint.cc │ ├── terrain_constraint.cc │ ├── phase_spline.cc │ └── node_spline.cc ├── include │ └── towr │ │ ├── models │ │ ├── endeffector_mappings.h │ │ ├── examples │ │ │ ├── monoped_model.h │ │ │ ├── biped_model.h │ │ │ ├── hyq_model.h │ │ │ ├── champ_model.h │ │ │ └── anymal_model.h │ │ ├── kinematic_model.h │ │ └── robot_model.h │ │ ├── variables │ │ ├── cartesian_dimensions.h │ │ ├── nodes_variables_all.h │ │ ├── variable_names.h │ │ ├── nodes_observer.h │ │ ├── phase_durations_observer.h │ │ ├── spline_holder.h │ │ ├── phase_spline.h │ │ └── spline.h │ │ ├── initialization │ │ ├── monoped_gait_generator.h │ │ ├── biped_gait_generator.h │ │ └── quadruped_gait_generator.h │ │ ├── costs │ │ ├── node_cost.h │ │ └── soft_constraint.h │ │ └── constraints │ │ ├── linear_constraint.h │ │ ├── spline_acc_constraint.h │ │ ├── total_duration_constraint.h │ │ ├── swing_constraint.h │ │ ├── base_motion_constraint.h │ │ ├── terrain_constraint.h │ │ ├── range_of_motion_constraint.h │ │ └── force_constraint.h └── CHANGELOG.rst ├── towr_ros ├── bash │ ├── README.md │ ├── remap_bag.sh │ ├── play_all.sh │ └── playbag.sh ├── launch │ ├── towr_nodes.launch │ └── towr_ros.launch ├── msg │ └── TowrCommand.msg ├── package.xml ├── include │ └── towr_ros │ │ ├── topic_names.h │ │ └── towr_user_interface.h ├── CHANGELOG.rst ├── src │ ├── goal_pose_publisher.cc │ ├── rosbag_geom_msg_extractor.cc │ └── rviz_terrain_publisher.cc └── CMakeLists.txt ├── LICENSE ├── CONTRIBUTING.md └── CODE_OF_CONDUCT.md /towr/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | doc/html/* 3 | doc/manifest.yaml 4 | -------------------------------------------------------------------------------- /towr/doc/nodes.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/nodes.jpg -------------------------------------------------------------------------------- /towr/doc/phase_nodes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/phase_nodes.png -------------------------------------------------------------------------------- /towr/doc/phase_nodes2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/phase_nodes2.png -------------------------------------------------------------------------------- /towr/doc/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox *.md' -------------------------------------------------------------------------------- /towr/doc/towr_motion_snaps.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/towr_motion_snaps.png -------------------------------------------------------------------------------- /towr/doc/towr_problem_with_code.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/towr_problem_with_code.png -------------------------------------------------------------------------------- /towr/doc/phase_based_parameterization.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/towr/HEAD/towr/doc/phase_based_parameterization.png -------------------------------------------------------------------------------- /towr_ros/bash/README.md: -------------------------------------------------------------------------------- 1 | Some useful scripts to playback and modify the produced ros-bag files. 2 | 3 | These are usually saved to: 4 | /home//.ros/towr_trajectory.bag 5 | -------------------------------------------------------------------------------- /towr_ros/bash/remap_bag.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #title :Remaps xpp/state_des in a bag file to another name 3 | #input :the name of the bag file 4 | #author :Alexander W. Winkler 5 | 6 | new_topic_name=xpp/state_curr 7 | bag_file=$1 8 | 9 | rosrun rosbag topic_renamer.py xpp/state_des ${bag_file}.bag $new_topic_name ${bag_file}_remap.bag 10 | 11 | 12 | -------------------------------------------------------------------------------- /towr/doc/mainpage.md: -------------------------------------------------------------------------------- 1 | TOWR - Trajectory Optimizer for Walking Robots {#mainpage} 2 | ================= 3 | *A light-weight and extensible C++ library for trajectory optimization for legged robots.* 4 | 5 | For an overview, see 6 | Github's README.md 7 | . 8 | 9 | \image html towr_motion_snaps.png 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /towr_ros/bash/play_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #title :Plays back all rosbag files in a specific folder 3 | #author :Alexander W. Winkler 4 | 5 | 6 | # change to directory where the script is stored so relative paths are correct 7 | my_dir="$(dirname "$0")" 8 | cd $my_dir 9 | 10 | speed=1.0 11 | 12 | video_folder=$my_dir/video1/* 13 | for bag_file in $video_folder 14 | do 15 | # draw the terrain 16 | rosbag play $bag_file \ 17 | --quiet \ 18 | --topics xpp/user_command_saved \ 19 | xpp/params xpp/user_command_saved:=/xpp/user_command \ 20 | -d 0.3 `#wait 0.01s after opening bag` \ 21 | 22 | # publish the paramters and terrain 23 | rosbag play $bag_file \ 24 | --topics xpp/state \ 25 | -r $speed 26 | done 27 | 28 | -------------------------------------------------------------------------------- /towr/matlab/com_motion_derivatives.m: -------------------------------------------------------------------------------- 1 | %% Derivatives of a 5th-order polynomial with respect to the coefficients 2 | % Author: Alexander Winkler 3 | 4 | clc; 5 | clear all; 6 | close all; 7 | 8 | syms a b c d e f t 9 | x = a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t + f; 10 | xd = diff(x, t); 11 | xdd = diff(x, t, 2); 12 | 13 | % Jacobian of these functions 14 | jac_x = jacobian(x, [a b c d e f]); 15 | jac_xd = jacobian(xd, [a b c d e f]); 16 | jac_xdd = jacobian(xdd, [a b c d e f]); 17 | 18 | % Jacobian of combined functions 19 | xd2 = xd^2; 20 | jac_xd2 = jacobian(xd^2, [a b c d e f]); 21 | fprintf('jacobian xd^2 wrt [a,b,c,d,e,f]:\n'); 22 | disp(simplify(jac_xd2)); 23 | 24 | jac_xd2 = jacobian(x*xd^2, [a b c d e f]); 25 | fprintf('jacobian x*xd^2 wrt [a,b,c,d,e,f]:\n'); 26 | disp(simplify(jac_xd2)); 27 | -------------------------------------------------------------------------------- /towr_ros/bash/playbag.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #title :Plays back rosbag files of optimization iterations 3 | #author :Alexander W. Winkler 4 | 5 | 6 | # change to directory where the script is stored to relative paths are correct 7 | my_dir="$(dirname "$0")" 8 | cd $my_dir 9 | 10 | speed=2.0 11 | 12 | # publish the optimization parameters, but wait 0.5s to make sure 13 | # subscribers are connected 14 | rosbag play towr_trajectory.bag --topics xpp/params -d 0.5 15 | 16 | 17 | # use rosbag API to remap the iteration topic to current robot state 18 | rosbag play towr_trajectory.bag \ 19 | --quiet \ 20 | --topics iter1 \ 21 | iter1:=/xpp/state \ 22 | -d 0.01 `#wait 0.01s after opening bag` \ 23 | -r $speed \ 24 | && \ 25 | rosbag play towr_trajectory.bag \ 26 | --topics iter10 \ 27 | -d 0.01 \ 28 | iter10:=/xpp/state \ 29 | -r $speed \ 30 | && \ 31 | rosbag play towr_trajectory.bag \ 32 | --topics iter20 \ 33 | -d 0.01 \ 34 | iter20:=/xpp/state \ 35 | -r $speed \ 36 | 37 | -------------------------------------------------------------------------------- /towr_ros/launch/towr_nodes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | 14 | 15 | 22 | 23 | 24 | 25 | 32 | 33 | 34 | 35 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /towr/matlab/gap_height_map.m: -------------------------------------------------------------------------------- 1 | %% Modelling of a gap in the terrain by a curved polynomial. 2 | % 3 | % Used in file height_map_examples.h to describe a gap terrain. 4 | % 5 | % Author: Alexander Winkler 6 | 7 | clc; 8 | clear all; 9 | 10 | % 3rd order poly in the shape of a terrain gap 11 | syms x a b c 12 | z = a*x^2 + b*x + c; 13 | 14 | 15 | syms h w xc 16 | % initial position (= terrain height) and velocity are zero 17 | x = xc; 18 | height_center = subs(z) == -h; 19 | 20 | 21 | % position at endge of gap should be zero (same height as started) 22 | x = xc-w/2; 23 | height_start = subs(z) == 0; 24 | x = xc+w/2; 25 | height_end = subs(z) == 0; 26 | 27 | 28 | % solve the system of equations for the polynomial coefficients 29 | S = solve([height_center, height_start, height_end], [a,b,c]); 30 | 31 | a = S.a 32 | b = S.b 33 | c = S.c 34 | 35 | syms x; 36 | pretty(subs(z)) 37 | subs(z) 38 | 39 | 40 | %% Plot the shape of the gap just to double check 41 | clc; 42 | clear all; 43 | 44 | h = 5.0; 45 | w = 0.25; 46 | xc = 1.2; 47 | x = [0.9:0.1:1.5]; 48 | 49 | % CAREFUL: copied from solution of first part 50 | height = (4*h*x.^2)/w^2 - (8*h*x*xc)/w^2 - (h*(w - 2*xc)*(w + 2*xc))/w^2; 51 | 52 | plot(x, height); -------------------------------------------------------------------------------- /towr_ros/msg/TowrCommand.msg: -------------------------------------------------------------------------------- 1 | # The command for the robot specified by the user 2 | 3 | xpp_msgs/StateLin3d goal_lin # the linear state to reach (x,y,z and derivatives) 4 | xpp_msgs/StateLin3d goal_ang # the angular state to reach (roll, pitch, yaw and derivatives) 5 | float64 total_duration # the total time to reach the goal state 6 | bool replay_trajectory # Replay the already optimized trajectory in RVIZ 7 | bool play_initialization # Play motion generated by unoptimized initial variables 8 | bool plot_trajectory # Plot the optimized trajectory 9 | float64 replay_speed # speed at which to playback the motion. 10 | bool optimize # run TOWR optimization 11 | int32 robot # Monoped, Biped, Quadruped, Anymal 12 | int32 terrain # some information about the used terrain (e.g stairs, gap, slope) 13 | int32 gait # Type of Motion (Walk, Trott, Bound, Pace) 14 | bool optimize_phase_durations # If true, the gait is optimized over as well 15 | -------------------------------------------------------------------------------- /towr/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | towr 4 | 1.4.1 5 | 6 | A light-weight, Eigen-based 7 | C++ library for trajectory optimization for legged robots. 8 | This library provides implementations for variables, costs and constraints 9 | that can be used to represent a legged locomotion problem. 10 | The resulting Nonlinear Programming Problem (NLP) can then be solved with 11 | off-the-shelf solvers, e.g. Ipopt using the generic optimizer interface 12 | ifopt. 13 | 14 | 15 | Alexander W. Winkler 16 | Alexander W. Winkler 17 | BSD 18 | 19 | http://github.com/ethz-adrl/towr 20 | http://github.com/ethz-adrl/towr/issues 21 | 22 | ifopt 23 | eigen 24 | 25 | 26 | catkin 27 | cmake 28 | 29 | cmake 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /towr_ros/launch/towr_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /towr_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | towr_ros 4 | 1.4.1 5 | 6 | A ROS dependent wrapper for 7 | towr. 8 | Adds a keyboard user interface to set different goal states, motions 9 | and robots and visualizes the produced motions plan in rviz using 10 | xpp. 11 | 12 | 13 | Alexander W. Winkler 14 | Alexander W. Winkler 15 | BSD 16 | 17 | http://github.com/ethz-adrl/towr 18 | http://github.com/ethz-adrl/towr/issues 19 | 20 | catkin 21 | 22 | libncurses-dev 23 | 24 | message_runtime 25 | message_generation 26 | std_msgs 27 | visualization_msgs 28 | roscpp 29 | rosbag 30 | xpp_states 31 | xpp_msgs 32 | towr 33 | 34 | xterm 35 | xpp_hyq 36 | rviz 37 | rqt_bag 38 | 39 | -------------------------------------------------------------------------------- /towr/cmake/towr-config.cmake: -------------------------------------------------------------------------------- 1 | # towr-config.cmake 2 | # --------- 3 | # 4 | # Locates the towr library and includes in cmake project. 5 | # Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 6 | # 7 | # 8 | # Imported targets 9 | # ^^^^^^^^^^^^^^^^ 10 | # 11 | # This module defines the following IMPORTED targets: 12 | # towr::towr - variables and constraints for legged locomotion 13 | # 14 | # 15 | # Example usage 16 | # ^^^^^^^^^^^^^ 17 | # 18 | # find_package(towr REQUIRED) 19 | # add_executable(foo my_problem.cc) 20 | # target_link_libraries(foo PUBLIC towr::towr) 21 | # 22 | # 23 | # Result variables 24 | # ^^^^^^^^^^^^^^^^ 25 | # 26 | # This module will set the following variables in your project. 27 | # These can be neccessary when building with catkin and not modern cmake 28 | # towr_INCLUDE_DIRS - path to public include (.h) files 29 | # towr_LIBRARIES - path to all libraries 30 | # 31 | #============================================================================= 32 | include(CMakeFindDependencyMacro) 33 | find_dependency(ifopt) 34 | 35 | # these are autogenerate by cmake 36 | include("${CMAKE_CURRENT_LIST_DIR}/towr-targets.cmake") 37 | 38 | get_target_property(towr_INCLUDE_DIRS towr::towr INTERFACE_INCLUDE_DIRECTORIES) 39 | list(APPEND towr_INCLUDE_DIRS ${ifopt_INCLUDE_DIRS}) 40 | 41 | get_property(towr_LIBRARIES TARGET towr::towr PROPERTY LOCATION) 42 | list(APPEND towr_LIBRARIES ${ifopt_LIBRARIES}) -------------------------------------------------------------------------------- /towr/matlab/euler_converter.m: -------------------------------------------------------------------------------- 1 | %% Derivatives of a Euler->Angular velocity mappings w.r.t euler angles. 2 | % 3 | % Used in class EulerConverter to specifcy analytical derivatives. 4 | % 5 | % Author: Alexander Winkler 6 | 7 | clc; 8 | clear all; 9 | 10 | syms t u % time and polynomial coefficients defining euler angles 11 | syms z(t,u) y(t,u) x(t,u) %euler angles yaw, pitch, roll 12 | 13 | % matrix that maps euler rates to angular velocities. 14 | % see https://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf 15 | M = [0, -sin(z), cos(y)*cos(z); 16 | 0, cos(z), cos(y)*sin(z); 17 | 1, 0, -sin(y)] 18 | 19 | dM_du = diff(M,u) 20 | 21 | 22 | 23 | %% Derivative of M_dot 24 | 25 | % Derivative of M w.r.t time 26 | Md = diff(M,t) 27 | 28 | % Derivative of M_dot w.r.t polynomial coefficients u defining euler angles 29 | dMd_du = diff(Md,u) 30 | 31 | 32 | 33 | %% derivative of rotation matrix defined by euler angles 34 | % from kindr cheet sheet, using convention zyx 35 | % see https://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf 36 | 37 | R = [cos(y)*cos(z), cos(z)*sin(x)*sin(y) - cos(x)*sin(z), sin(x)*sin(z) + cos(x)*cos(z)*sin(y); 38 | cos(y)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(x)*sin(y)*sin(z) - cos(z)*sin(x); 39 | -sin(y), cos(y)*sin(x), cos(x)*cos(y)] 40 | 41 | 42 | dR_du = diff(R,u) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Alexander W. Winkler 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /towr/matlab/cubic_hermite_polynomial.m: -------------------------------------------------------------------------------- 1 | %% Provides derivatives of a third-order polynomial w.r.t. nodes 2 | % 3 | % These results are used in class CubicHermitePolynomial to code the 4 | % analytical derivatives. 5 | % 6 | % Author: Alexander Winkler 7 | 8 | clc; 9 | clear all; 10 | 11 | 12 | %% Define the polynomial through its coefficients 13 | syms a b c d % the polynomial coefficients 14 | syms p0 v0 p1 v1 % the initial and final position and velocity ("nodes") 15 | syms t % the current time 16 | syms T % The total duration of the polynomial 17 | 18 | % a third-order polynomial and it's derivatives 19 | pos = d*t^3 + c*t^2 + b*t + a; 20 | vel = diff(pos, t); 21 | acc = diff(pos, t, 2); 22 | 23 | 24 | %% Express polynomial coefficients as functions of node values/duration 25 | % This fully defines the 3rd-order polynomial and is called Hermite 26 | % Parameterization. 27 | 28 | % initial position and velocity are zero 29 | t = 0; 30 | eq_init_p = subs(pos) == p0; 31 | eq_init_v = subs(vel) == v0; 32 | 33 | 34 | % final position and velocity are zero (at time t=1) 35 | t = T; 36 | eq_final_p = subs(pos) == p1; 37 | eq_final_v = subs(vel) == v1; 38 | 39 | 40 | % solve the system of equations for the polynomial coefficients 41 | S = solve([eq_init_p, eq_init_v, eq_final_p, eq_final_v], [a,b,c,d]); 42 | 43 | a = S.a; 44 | b = S.b; 45 | c = S.c; 46 | d = S.d; 47 | 48 | syms t; 49 | 50 | 51 | %% Derivative of pos, vel and acc w.r.t the node values and duration 52 | % position 53 | jac_p = jacobian(subs(pos), [p0 v0 p1 v1 T]); 54 | 55 | dp_dp0 = jac_p(1) 56 | dp_dv0 = jac_p(2) 57 | dp_dp1 = jac_p(3) 58 | dp_dv1 = jac_p(4) 59 | dp_dT = jac_p(5) 60 | 61 | % velocity 62 | jac_v = jacobian(subs(vel), [p0 v0 p1 v1 T]) 63 | 64 | dpv_dp0 = jac_v(1) 65 | dpv_dv0 = jac_v(2) 66 | dpv_dp1 = jac_v(3) 67 | dpv_dv1 = jac_v(4) 68 | dpv_dT = jac_v(5) 69 | 70 | % acceleration 71 | jac_a = jacobian(subs(acc), [p0 v0 p1 v1 T]) 72 | 73 | dpa_dp0 = jac_a(1) 74 | dpa_dv0 = jac_a(2) 75 | dpa_dp1 = jac_a(3) 76 | dpa_dv1 = jac_a(4) 77 | dpa_dT = jac_a(5) -------------------------------------------------------------------------------- /towr/test/dynamic_constraint_test.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace towr { 36 | 37 | using VectorXd = Eigen::VectorXd; 38 | 39 | 40 | TEST(DynamicConstraintTest, UpdateConstraintValues) 41 | { 42 | // add dynamic constraint test 43 | } 44 | 45 | } /* namespace towr */ 46 | -------------------------------------------------------------------------------- /towr/src/nodes_observer.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | namespace towr { 34 | 35 | NodesObserver::NodesObserver(NodeSubjectPtr subject) 36 | { 37 | node_values_ = subject; 38 | 39 | // register observer to subject so this class always up-to-date 40 | subject->AddObserver(this); 41 | }; 42 | 43 | } // namespace towr 44 | 45 | 46 | -------------------------------------------------------------------------------- /towr/test/dynamic_model_test.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace towr { 35 | 36 | TEST(DynamicModelTest, GetBaseAcceleration) 37 | { 38 | // update test 39 | } 40 | 41 | TEST(DynamicModelTest, GetJacobianOfAccWrtBase) 42 | { 43 | // update test 44 | } 45 | 46 | TEST(DynamicModelTest, TestRotations) 47 | { 48 | // update test 49 | } 50 | 51 | } /* namespace xpp */ 52 | -------------------------------------------------------------------------------- /towr/src/phase_durations_observer.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace towr { 35 | 36 | 37 | PhaseDurationsObserver::PhaseDurationsObserver (PhaseDurationsSubjectPtr subject) 38 | { 39 | phase_durations_ = subject; 40 | 41 | // register this observer to subject so this class always up-to-date 42 | subject->AddObserver(this); 43 | } 44 | 45 | } // namespace towr 46 | 47 | -------------------------------------------------------------------------------- /towr/include/towr/models/endeffector_mappings.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | /** 31 | * @file endeffector_mappings.h 32 | * 33 | * Assigning some semantic information (e.g. name of the foot) to endeffector 34 | * indices. This is only needed to later visualize each endeffector with 35 | * the correct URDF legs. 36 | */ 37 | #ifndef TOWR_ENDEFFECTOR_MAPPINGS_H_ 38 | #define TOWR_ENDEFFECTOR_MAPPINGS_H_ 39 | 40 | namespace towr { 41 | 42 | // has to start at zero 43 | enum BipedIDs { L, R }; 44 | enum QuadrupedIDs { LF, RF, LH, RH }; 45 | 46 | } // namespace towr 47 | 48 | #endif /* TOWR_STATES_ENDEFFECTOR_MAPPINGS_H_ */ 49 | -------------------------------------------------------------------------------- /towr_ros/include/towr_ros/topic_names.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_ROS_TOPIC_NAMES_H_ 31 | #define TOWR_ROS_TOPIC_NAMES_H_ 32 | 33 | #include 34 | 35 | namespace towr_msgs { 36 | 37 | // position of the desired goal to reach 38 | static const std::string user_command("/towr/user_command"); 39 | 40 | // iterations the nlp took to solve the problem. Used when processing rosbags 41 | static const std::string nlp_iterations_count("/towr/nlp_iterations_count"); 42 | 43 | // the base topic names of each nlp iteration 44 | static const std::string nlp_iterations_name("/towr/nlp_iterations_name"); 45 | 46 | } // namespace towr_msgs 47 | 48 | 49 | #endif /* TOWR_ROS_TOPIC_NAMES_H_ */ 50 | -------------------------------------------------------------------------------- /towr/src/state.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | 33 | namespace towr { 34 | 35 | State::State (int dim, int n_derivatives) 36 | { 37 | values_ = std::vector(n_derivatives, VectorXd::Zero(dim)); 38 | } 39 | 40 | const Eigen::VectorXd 41 | State::at (Dx deriv) const 42 | { 43 | return values_.at(deriv); 44 | } 45 | 46 | Eigen::VectorXd& 47 | State::at (Dx deriv) 48 | { 49 | return values_.at(deriv); 50 | } 51 | 52 | const Eigen::VectorXd 53 | State::p () const 54 | { 55 | return at(kPos); 56 | } 57 | 58 | const Eigen::VectorXd 59 | State::v () const 60 | { 61 | return at(kVel); 62 | } 63 | 64 | const Eigen::VectorXd 65 | State::a () const 66 | { 67 | return at(kAcc); 68 | } 69 | 70 | } // namespace towr 71 | 72 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | 2 | # Contributing to towr 3 | 4 | Thanks for taking the time to contribute! :tada::+1: 5 | 6 | We are grateful to anyone who decides to actively engage in this project. 7 | This project and everyone participating in it is governed by the [Code of Conduct](CODE_OF_CONDUCT.md). 8 | By participating, you are expected to uphold this code. 9 | 10 | 11 | ## Question or Issue? 12 | * [ROS Answers](https://answers.ros.org/questions/scope:all/sort:activity-desc/tags:towr/page:1/): 13 | If you want to ask a general usage question, please do this at ROS Answers using the tag `towr`. Asking there has the following advantages: 14 | 15 | - There are many more people reading questions there so your chances on getting an answer and in a timely manner are much higher. 16 | - In the future other users will search there for similar problems and can find your question and the potential answers. 17 | 18 | * [Issue tracker](https://github.com/ethz-adrl/towr/issues): 19 | In case you want to report bugs, request features or anything closely related to the code. 20 | 21 | 22 | ## How Can I Contribute? 23 | Unsure where to begin? You can start by looking through these issues: 24 | 25 | * [Good first issues](https://github.com/ethz-adrl/towr/issues?q=is%3Aopen+is%3Aissue+label%3A%22good+first+issue%22) - issues which should only require a few lines of code, and a test or two. 26 | * [Authors recommendations](https://github.com/ethz-adrl/towr/issues?q=is%3Aopen+is%3Aissue+label%3A%22authors+recommendations%22) - advanced issues that can possibly bring great improvement. 27 | 28 | Further ideas might include: 29 | - add cool terrains/height-maps 30 | - add different robot models 31 | - enhance the user interface 32 | - add additional constraints 33 | - improve performance 34 | - improve documentation 35 | 36 | Please follow the [Google Style Guide](https://google.github.io/styleguide/cppguide.html) when writing your C++ Code for this project. 37 | 38 | 39 | ## Submitting a pull request 40 | Please create a [pull request](https://github.com/ethz-adrl/towr/compare) from your forked repo to the master branch. 41 | 42 | - Keep pull requests small, focussing on one specific issue. 43 | - Describe concisely what you are solving and reference the corresponding issue. 44 | - Include weblinks to relevant information. 45 | - Include screenshots and animated GIFs in your pull request whenever possible. 46 | 47 | We are performing automatic pull request testing, so if these fail, please read through the log and update your pull request. 48 | 49 | -------------------------------------------------------------------------------- /towr/src/dynamic_model.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace towr { 33 | 34 | DynamicModel::DynamicModel(double mass, int ee_count) 35 | { 36 | m_ = mass; 37 | g_ = 9.80665; 38 | 39 | com_pos_.setZero(); 40 | com_acc_.setZero(); 41 | 42 | w_R_b_.setIdentity(); 43 | omega_.setZero(); 44 | omega_dot_ .setZero(); 45 | 46 | ee_force_ = EELoad(ee_count); 47 | ee_pos_ = EEPos(ee_count); 48 | } 49 | 50 | void 51 | DynamicModel::SetCurrent (const ComPos& com_W, const Vector3d com_acc_W, 52 | const Matrix3d& w_R_b, const AngVel& omega_W, const Vector3d& omega_dot_W, 53 | const EELoad& force_W, const EEPos& pos_W) 54 | { 55 | com_pos_ = com_W; 56 | com_acc_ = com_acc_W; 57 | 58 | w_R_b_ = w_R_b; 59 | omega_ = omega_W; 60 | omega_dot_ = omega_dot_W; 61 | 62 | ee_force_ = force_W; 63 | ee_pos_ = pos_W; 64 | } 65 | 66 | } /* namespace towr */ 67 | -------------------------------------------------------------------------------- /towr/include/towr/variables/cartesian_dimensions.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | /** 31 | * @file cartesian_declartions.h 32 | * 33 | * Defines common conventions and index values to be used in Cartesian 34 | * environments. 35 | */ 36 | #ifndef TOWR_VARIABLES_CARTESIAN_DIMENSIONS_H_ 37 | #define TOWR_VARIABLES_CARTESIAN_DIMENSIONS_H_ 38 | 39 | #include 40 | 41 | namespace towr { 42 | 43 | // 2-dimensional 44 | static constexpr int k2D = 2; 45 | enum Dim2D { X_=0, Y_}; 46 | 47 | // 3-dimensional 48 | static constexpr int k3D = 3; 49 | enum Dim3D { X=0, Y, Z }; 50 | static Dim2D To2D(Dim3D dim) 51 | { 52 | assert(dim != Z); 53 | return static_cast(dim); 54 | }; 55 | 56 | // 6-dimensional 57 | // 'A' stands for angular, 'L' for linear. 58 | static constexpr int k6D = 6; // X,Y,Z, roll, pitch, yaw 59 | enum Dim6D { AX=0, AY, AZ, LX, LY, LZ }; 60 | static const Dim6D AllDim6D[] = {AX, AY, AZ, LX, LY, LZ}; 61 | 62 | } // namespace towr 63 | 64 | #endif /* TOWR_VARIABLES_CARTESIAN_DIMENSIONS_H_ */ 65 | -------------------------------------------------------------------------------- /towr/include/towr/initialization/monoped_gait_generator.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_MODELS_MONOPED_GAIT_GENERATOR_H_ 31 | #define TOWR_MODELS_MONOPED_GAIT_GENERATOR_H_ 32 | 33 | #include "gait_generator.h" 34 | 35 | namespace towr { 36 | 37 | /** 38 | * @brief Produces the contact sequence for a variety of one-legged gaits. 39 | * 40 | * @sa GaitGenerator for more documentation 41 | */ 42 | class MonopedGaitGenerator : public GaitGenerator { 43 | public: 44 | MonopedGaitGenerator () = default; 45 | virtual ~MonopedGaitGenerator () = default; 46 | 47 | private: 48 | GaitInfo GetGait(Gaits gait) const override; 49 | 50 | GaitInfo GetStrideStand() const; 51 | GaitInfo GetStrideFlight() const; 52 | GaitInfo GetStrideHop() const; 53 | GaitInfo GetStrideHopLong() const; 54 | 55 | ContactState o_ = ContactState(1, true); // stance 56 | ContactState x_ = ContactState(1, false); // flight 57 | 58 | void SetCombo(Combos combo) override; 59 | }; 60 | 61 | } /* namespace towr */ 62 | 63 | #endif /* TOWR_MODELS_MONOPED_GAIT_GENERATOR_H_ */ 64 | -------------------------------------------------------------------------------- /towr/src/nodes_variables_all.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace towr { 33 | 34 | NodesVariablesAll::NodesVariablesAll (int n_nodes, int n_dim, std::string variable_id) 35 | : NodesVariables(variable_id) 36 | { 37 | int n_opt_variables = n_nodes*Node::n_derivatives*n_dim; 38 | 39 | n_dim_ = n_dim; 40 | nodes_ = std::vector(n_nodes, Node(n_dim)); 41 | bounds_ = VecBound(n_opt_variables, ifopt::NoBound); 42 | SetRows(n_opt_variables); 43 | } 44 | 45 | std::vector 46 | NodesVariablesAll::GetNodeValuesInfo (int idx) const 47 | { 48 | std::vector vec_nvi; 49 | 50 | int n_opt_values_per_node_ = 2*GetDim(); 51 | int internal_id = idx%n_opt_values_per_node_; // 0...6 (p.x, p.y, p.z, v.x, v.y. v.z) 52 | 53 | NodeValueInfo nvi; 54 | nvi.deriv_ = internal_id GetNodeValuesInfo(int idx) const override; 56 | }; 57 | 58 | } /* namespace towr */ 59 | 60 | #endif /* TOWR_TOWR_INCLUDE_TOWR_VARIABLES_BASE_NODES_H_ */ 61 | -------------------------------------------------------------------------------- /towr/include/towr/variables/variable_names.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_VARIABLES_VARIABLE_NAMES_H_ 31 | #define TOWR_VARIABLES_VARIABLE_NAMES_H_ 32 | 33 | #include 34 | 35 | namespace towr { 36 | /** 37 | * @brief Identifiers (names) used for variables in the optimization problem. 38 | * 39 | * @ingroup Variables 40 | */ 41 | namespace id { 42 | 43 | static const std::string base_lin_nodes = "base-lin"; 44 | static const std::string base_ang_nodes = "base-ang"; 45 | static const std::string ee_motion_nodes = "ee-motion_"; 46 | static const std::string ee_force_nodes = "ee-force_"; 47 | static const std::string contact_schedule = "ee-schedule"; 48 | 49 | 50 | static std::string EEMotionNodes(uint ee) 51 | { 52 | return ee_motion_nodes + std::to_string(ee); 53 | } 54 | 55 | static std::string EEForceNodes(uint ee) 56 | { 57 | return ee_force_nodes + std::to_string(ee); 58 | } 59 | 60 | static std::string EESchedule(uint ee) 61 | { 62 | return contact_schedule + std::to_string(ee); 63 | } 64 | 65 | } // namespace id 66 | } // namespace towr 67 | 68 | 69 | 70 | #endif /* TOWR_VARIABLES_VARIABLE_NAMES_H_ */ 71 | -------------------------------------------------------------------------------- /towr/include/towr/models/examples/monoped_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_MONOPED_MODEL_H_ 31 | #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_MONOPED_MODEL_H_ 32 | 33 | #include 34 | #include 35 | 36 | namespace towr { 37 | 38 | /** 39 | * @brief The Kinematics of a one-legged hopper with HyQ leg. 40 | */ 41 | class MonopedKinematicModel : public KinematicModel { 42 | public: 43 | MonopedKinematicModel () : KinematicModel(1) 44 | { 45 | nominal_stance_.at(0) = Eigen::Vector3d( 0.0, 0.0, -0.58); 46 | max_dev_from_nominal_ << 0.25, 0.15, 0.2; 47 | } 48 | }; 49 | 50 | /** 51 | * @brief The Dynamics of a one-legged hopper with HyQ leg. 52 | */ 53 | class MonopedDynamicModel : public SingleRigidBodyDynamics { 54 | public: 55 | MonopedDynamicModel() 56 | : SingleRigidBodyDynamics(20, // mass of the robot 57 | 1.2, 5.5, 6.0, 0.0, -0.2, -0.01, // base inertia 58 | 1) {} // number of endeffectors 59 | }; 60 | 61 | } /* namespace towr */ 62 | 63 | #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_MONOPED_MODEL_H_ */ 64 | -------------------------------------------------------------------------------- /towr/src/soft_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace towr { 33 | 34 | SoftConstraint::SoftConstraint (const ConstraintPtr& constraint) 35 | :Component(1, "soft-" + constraint->GetName()) 36 | { 37 | constraint_ = constraint; 38 | int n_constraints = constraint_->GetRows(); 39 | 40 | // average value of each upper and lower bound 41 | b_ = VectorXd(n_constraints); 42 | int i=0; 43 | for (auto b : constraint_->GetBounds()) { 44 | b_(i++) = (b.upper_ + b.lower_)/2.; 45 | } 46 | 47 | // treat all constraints equally by default 48 | W_.resize(n_constraints); 49 | W_.setOnes(); 50 | } 51 | 52 | SoftConstraint::VectorXd 53 | SoftConstraint::GetValues () const 54 | { 55 | VectorXd g = constraint_->GetValues(); 56 | VectorXd cost = 0.5*(g-b_).transpose()*W_.asDiagonal()*(g-b_); 57 | return cost; 58 | } 59 | 60 | SoftConstraint::Jacobian 61 | SoftConstraint::GetJacobian () const 62 | { 63 | VectorXd g = constraint_->GetValues(); 64 | Jacobian jac = constraint_->GetJacobian(); 65 | VectorXd grad = jac.transpose()*W_.asDiagonal()*(g-b_); 66 | return grad.transpose().sparseView(); 67 | } 68 | 69 | } /* namespace towr */ 70 | -------------------------------------------------------------------------------- /towr/include/towr/models/examples/biped_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_BIPED_MODEL_H_ 31 | #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_BIPED_MODEL_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief The Kinematics of a tow-legged robot built from HyQ legs. 41 | */ 42 | class BipedKinematicModel : public KinematicModel { 43 | public: 44 | BipedKinematicModel () : KinematicModel(2) 45 | { 46 | const double z_nominal_b = -0.65; 47 | const double y_nominal_b = 0.20; 48 | 49 | nominal_stance_.at(L) << 0.0, y_nominal_b, z_nominal_b; 50 | nominal_stance_.at(R) << 0.0, -y_nominal_b, z_nominal_b; 51 | 52 | max_dev_from_nominal_ << 0.25, 0.15, 0.15; 53 | } 54 | }; 55 | 56 | /** 57 | * @brief The Dynamics of a tow-legged robot built from HyQ legs. 58 | */ 59 | class BipedDynamicModel : public SingleRigidBodyDynamics { 60 | public: 61 | BipedDynamicModel() 62 | : SingleRigidBodyDynamics(20, 63 | 1.209,5.583,6.056,0.005,-0.190,-0.012, 64 | 2) {} 65 | }; 66 | 67 | } /* namespace towr */ 68 | 69 | #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_BIPED_MODEL_H_ */ 70 | -------------------------------------------------------------------------------- /towr/src/linear_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace towr { 33 | 34 | 35 | LinearEqualityConstraint::LinearEqualityConstraint ( 36 | const Eigen::MatrixXd& M, 37 | const Eigen::VectorXd& v, 38 | const std::string& variable_name) 39 | : ConstraintSet(v.rows(), "linear-equality-" + variable_name) 40 | { 41 | M_ = M; 42 | v_ = v; 43 | variable_name_ = variable_name; 44 | } 45 | 46 | LinearEqualityConstraint::VectorXd 47 | LinearEqualityConstraint::GetValues () const 48 | { 49 | VectorXd x = GetVariables()->GetComponent(variable_name_)->GetValues(); 50 | return M_*x; 51 | } 52 | 53 | LinearEqualityConstraint::VecBound 54 | LinearEqualityConstraint::GetBounds () const 55 | { 56 | VecBound bounds; 57 | 58 | for (int i=0; i 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | 41 | namespace towr { 42 | 43 | /** 44 | * @brief Assigns a cost to node values. 45 | * 46 | * @ingroup Costs 47 | */ 48 | class NodeCost : public ifopt::CostTerm { 49 | public: 50 | /** 51 | * @brief Constructs a cost term for the optimization problem. 52 | * @param nodes_id The name of the node variables. 53 | * @param deriv The node derivative (pos, vel) which should be penalized. 54 | * @param dim The node dimension which should be penalized. 55 | */ 56 | NodeCost (const std::string& nodes_id, Dx deriv, int dim, double weight); 57 | virtual ~NodeCost () = default; 58 | 59 | void InitVariableDependedQuantities(const VariablesPtr& x) override; 60 | 61 | double GetCost () const override; 62 | 63 | private: 64 | std::shared_ptr nodes_; 65 | 66 | std::string node_id_; 67 | Dx deriv_; 68 | int dim_; 69 | double weight_; 70 | 71 | void FillJacobianBlock(std::string var_set, Jacobian&) const override; 72 | }; 73 | 74 | } /* namespace towr */ 75 | 76 | #endif /* TOWR_COSTS_NODE_COST_H_ */ 77 | -------------------------------------------------------------------------------- /towr/include/towr/initialization/biped_gait_generator.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_MODELS_BIPED_GAIT_GENERATOR_H_ 31 | #define TOWR_MODELS_BIPED_GAIT_GENERATOR_H_ 32 | 33 | #include "gait_generator.h" 34 | 35 | namespace towr { 36 | 37 | /** 38 | * @brief Produces the contact sequence for a variety of two-legged gaits. 39 | * 40 | * @sa GaitGenerator for more documentation 41 | */ 42 | class BipedGaitGenerator : public GaitGenerator { 43 | public: 44 | BipedGaitGenerator (); 45 | virtual ~BipedGaitGenerator () = default; 46 | 47 | private: 48 | GaitInfo GetGait(Gaits gait) const override; 49 | 50 | GaitInfo GetStrideStand() const; 51 | GaitInfo GetStrideFlight() const; 52 | GaitInfo GetStrideWalk() const; 53 | GaitInfo GetStrideRun() const; 54 | GaitInfo GetStrideHop() const; 55 | GaitInfo GetStrideLeftHop() const; 56 | GaitInfo GetStrideRightHop() const; 57 | GaitInfo GetStrideGallopHop() const; 58 | 59 | void SetCombo(Combos combo) override; 60 | 61 | // naming convention:, where the circle is is contact, front is right ->. 62 | ContactState I_; // flight 63 | ContactState b_; // right-leg in contact 64 | ContactState P_; // left leg in contact 65 | ContactState B_; // stance (both legs in contact) 66 | }; 67 | 68 | } /* namespace towr */ 69 | 70 | #endif /* TOWR_MODELS_BIPED_GAIT_GENERATOR_H_ */ 71 | -------------------------------------------------------------------------------- /towr/src/node_cost.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace towr { 35 | 36 | NodeCost::NodeCost (const std::string& nodes_id, Dx deriv, int dim, double weight) 37 | : CostTerm(nodes_id 38 | +"-dx_"+std::to_string(deriv) 39 | +"-dim_"+std::to_string(dim)) 40 | { 41 | node_id_ = nodes_id; 42 | deriv_ = deriv; 43 | dim_ = dim; 44 | weight_ = weight; 45 | } 46 | 47 | void 48 | NodeCost::InitVariableDependedQuantities (const VariablesPtr& x) 49 | { 50 | nodes_ = x->GetComponent(node_id_); 51 | } 52 | 53 | double 54 | NodeCost::GetCost () const 55 | { 56 | double cost; 57 | for (auto n : nodes_->GetNodes()) { 58 | double val = n.at(deriv_)(dim_); 59 | cost += weight_*std::pow(val,2); 60 | } 61 | 62 | return cost; 63 | } 64 | 65 | void 66 | NodeCost::FillJacobianBlock (std::string var_set, Jacobian& jac) const 67 | { 68 | if (var_set == node_id_) { 69 | for (int i=0; iGetRows(); ++i) 70 | for (auto nvi : nodes_->GetNodeValuesInfo(i)) 71 | if (nvi.deriv_==deriv_ && nvi.dim_==dim_) { 72 | double val = nodes_->GetNodes().at(nvi.id_).at(deriv_)(dim_); 73 | jac.coeffRef(0, i) += weight_*2.0*val; 74 | } 75 | } 76 | } 77 | 78 | } /* namespace towr */ 79 | 80 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/linear_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_LINEAR_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_LINEAR_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | namespace towr { 36 | 37 | /** 38 | * @brief Calculates the constraint violations for linear constraints. 39 | * 40 | * @ingroup Constraints 41 | */ 42 | class LinearEqualityConstraint : public ifopt::ConstraintSet { 43 | public: 44 | using MatrixXd = Eigen::MatrixXd; 45 | 46 | /** 47 | * @brief Defines the elements of the linear constraint as g = Mx+v = 0. 48 | * 49 | * @param M The matrix M defining the slope. 50 | * @param v The vector v defining the constanct offset. 51 | * @param variable_set The name of the variables x. 52 | */ 53 | LinearEqualityConstraint (const MatrixXd& M, 54 | const VectorXd& v, 55 | const std::string& variable_set); 56 | virtual ~LinearEqualityConstraint () = default; 57 | 58 | VectorXd GetValues() const final; 59 | VecBound GetBounds() const final; 60 | void FillJacobianBlock (std::string var_set, Jacobian&) const final; 61 | 62 | private: 63 | MatrixXd M_; 64 | VectorXd v_; 65 | std::string variable_name_; 66 | }; 67 | 68 | } /* namespace towr */ 69 | 70 | #endif /* TOWR_CONSTRAINTS_LINEAR_CONSTRAINT_H_ */ 71 | -------------------------------------------------------------------------------- /towr/include/towr/variables/nodes_observer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_NODES_OBSERVER_H_ 31 | #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_NODES_OBSERVER_H_ 32 | 33 | 34 | namespace towr { 35 | 36 | class NodesVariables; 37 | 38 | /** 39 | * @brief Base class to receive up-to-date values of the NodeVariables. 40 | * 41 | * This class registers with the node variables and everytime the positions or 42 | * velocities of a node change, the subject updates this class by calling the 43 | * UpdatePolynomials() method. 44 | * 45 | * Used by spline.h 46 | * 47 | * This class implements the observer pattern: 48 | * https://sourcemaking.com/design_patterns/observer 49 | */ 50 | class NodesObserver { 51 | public: 52 | using NodeSubjectPtr = NodesVariables*; // observer shouldn't own subject 53 | 54 | /** 55 | * @brief Registers this observer with the subject class to receive updates. 56 | * @param node_values The subject holding the Hermite node values. 57 | */ 58 | NodesObserver(NodeSubjectPtr node_values); 59 | virtual ~NodesObserver() = default; 60 | 61 | /** 62 | * @brief Callback method called every time the subject changes. 63 | */ 64 | virtual void UpdateNodes() = 0; 65 | 66 | protected: 67 | NodeSubjectPtr node_values_; 68 | }; 69 | 70 | } /* namespace towr */ 71 | 72 | #endif /* TOWR_TOWR_INCLUDE_TOWR_VARIABLES_NODES_OBSERVER_H_ */ 73 | -------------------------------------------------------------------------------- /towr_ros/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package towr_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2019-04-05) 6 | ------------------ 7 | * Merge pull request (`#56 `_) from ethz-adrl/expose-params 8 | * Contributors: Alexander Winkler, awinkler 9 | 10 | 1.4.0 (2018-07-30) 11 | ------------------ 12 | * Facilitate towr_ros user extension (`#34 `_) 13 | * Add easy plotting of trajectories with rqt_bag 14 | * add option to visualize variable initialization 15 | * Allow shorthand to launch with gdb 16 | * Contributors: Alexander Winkler 17 | 18 | 1.3.2 (2018-07-17) 19 | ------------------ 20 | * adapt to more generic ifopt solver interface. 21 | * Contributors: Alexander Winkler 22 | 23 | 1.3.1 (2018-07-10) 24 | ------------------ 25 | * Improve API (`#23 `_) 26 | * add gait optimization and replay speed to UI 27 | * Add ROS and codefactor badges to readme (`#22 `_) 28 | * Contributors: Alexander Winkler 29 | 30 | 1.3.0 (2018-07-07) 31 | ------------------ 32 | * visualize goal state on terrain 33 | * Remove redundant rviz terrain visualizer and instead generate 34 | surface patches directly from height map information 35 | * correctly visualize rviz range-of-motion boxes 36 | * visualize first state 37 | * make GUI static 38 | * Contributors: Alexander Winkler 39 | 40 | 1.2.2 (2018-07-03) 41 | ------------------ 42 | * remove controller specifc code from towr_ros 43 | * removed exe subfolder 44 | * moved height map from towr_ros to towr 45 | * moved robots models and gait generator from towr_ros to towr 46 | * move dynamic and kinematic models from towr_ros -> towr 47 | * add ncurses and xterm dependencies to package.xml 48 | * Contributors: Alexander Winkler 49 | 50 | 1.2.1 (2018-06-30) 51 | ------------------ 52 | * adapt to revised ifopt version and build structure 53 | * preparation for ANYmal experiments 54 | * Contributors: Alexander Winkler 55 | 56 | 1.2.0 (2018-06-25) 57 | ------------------ 58 | * adapt to version 2.0.0 of ifopt (`#17 `_) 59 | * fix discrepancy between gap height map and rviz visualization 60 | * Contributors: Alexander Winkler 61 | 62 | 1.1.0 (2018-02-06) 63 | ------------------ 64 | * improve launch files and class names 65 | * add metapackage towr and move algorithm to towr_core 66 | * create separate ros independent example package "towr_examples" 67 | * adapt to separated ifopt packages 68 | * replaced ros-keyboard dependency with ncurses 69 | * clean-up matlab scripts 70 | * clean and document towr_ros 71 | * add documentaton to angular_state_converter->euler_converter 72 | * moved all robot specific model/gait generators out of towr -> towr_ros 73 | * use only one unified represenatation for nodes and states 74 | * added spline_holder to not always have to reconstruct from variables 75 | * separated spline and node values 76 | * changed to different namespace towr 77 | * renamed pkg from xpp_opt to towr 78 | * Contributors: Alexander Winkler 79 | 80 | 1.0.0 (2017-09-19) 81 | ------------------ 82 | -------------------------------------------------------------------------------- /towr/include/towr/models/examples/hyq_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_HYQ_MODEL_H_ 31 | #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_HYQ_MODEL_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief The Kinematics of the quadruped robot HyQ. 41 | */ 42 | class HyqKinematicModel : public KinematicModel { 43 | public: 44 | HyqKinematicModel () : KinematicModel(4) 45 | { 46 | const double x_nominal_b = 0.31; 47 | const double y_nominal_b = 0.29; 48 | const double z_nominal_b = -0.58; 49 | 50 | nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; 51 | nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; 52 | nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; 53 | nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; 54 | 55 | max_dev_from_nominal_ << 0.25, 0.20, 0.10; 56 | } 57 | }; 58 | 59 | /** 60 | * @brief The Dynamics of the quadruped robot HyQ. 61 | */ 62 | class HyqDynamicModel : public SingleRigidBodyDynamics { 63 | public: 64 | HyqDynamicModel() : SingleRigidBodyDynamics(83, 65 | 4.26, 8.97, 9.88, -0.0063, 0.193, 0.0126, 66 | 4) {} 67 | }; 68 | 69 | } /* namespace towr */ 70 | 71 | #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_HYQ_MODEL_H_ */ 72 | -------------------------------------------------------------------------------- /towr/src/total_duration_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | namespace towr { 34 | 35 | 36 | TotalDurationConstraint::TotalDurationConstraint (double T_total, int ee) 37 | :ConstraintSet(1, "totalduration-" + std::to_string(ee)) 38 | { 39 | T_total_ = T_total; 40 | ee_ = ee; 41 | } 42 | 43 | void 44 | TotalDurationConstraint::InitVariableDependedQuantities (const VariablesPtr& x) 45 | { 46 | phase_durations_ = x->GetComponent(id::EESchedule(ee_)); 47 | } 48 | 49 | Eigen::VectorXd 50 | TotalDurationConstraint::GetValues () const 51 | { 52 | VectorXd g = VectorXd::Zero(GetRows()); 53 | g(0) = phase_durations_->GetValues().sum(); // attention: excludes last duration 54 | return g; 55 | } 56 | 57 | TotalDurationConstraint::VecBound 58 | TotalDurationConstraint::GetBounds () const 59 | { 60 | // TODO hacky and should be fixed 61 | // since last phase is not optimized over these hardcoded numbers go here 62 | int min_duration_last_phase = 0.2; 63 | return VecBound(GetRows(), ifopt::Bounds(0.1, T_total_-min_duration_last_phase)); 64 | } 65 | 66 | void 67 | TotalDurationConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const 68 | { 69 | if (var_set == phase_durations_->GetName()) 70 | for (int col=0; colGetRows(); ++col) 71 | jac.coeffRef(0, col) = 1.0; 72 | } 73 | 74 | } // namespace towr 75 | -------------------------------------------------------------------------------- /towr/include/towr/models/examples/champ_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_CHAMP_MODEL_H_ 31 | #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_CHAMP_MODEL_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief The Kinematics of the quadruped robot Champ. 41 | */ 42 | class ChampKinematicModel : public KinematicModel { 43 | public: 44 | ChampKinematicModel () : KinematicModel(4) 45 | { 46 | const double x_nominal_b = 0.175; 47 | const double y_nominal_b = 0.165; 48 | const double z_nominal_b = -0.21; 49 | 50 | nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; 51 | nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; 52 | nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; 53 | nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; 54 | 55 | max_dev_from_nominal_ << 0.08, 0.05, 0.05; 56 | } 57 | }; 58 | 59 | /** 60 | * @brief The Dynamics of the quadruped robot Champ. 61 | */ 62 | class ChampDynamicModel : public SingleRigidBodyDynamics { 63 | public: 64 | ChampDynamicModel() 65 | : SingleRigidBodyDynamics(3.31, 66 | 0.04, 0.08, 0.10, 0.0, 0.0, 0.0, 67 | 4) {} 68 | }; 69 | 70 | } // namespace towr 71 | 72 | #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_CHAMP_MODEL_H_ */ 73 | -------------------------------------------------------------------------------- /towr/src/robot_model.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace towr { 39 | 40 | 41 | RobotModel::RobotModel(Robot robot) 42 | { 43 | switch (robot) { 44 | case Monoped: 45 | dynamic_model_ = std::make_shared(); 46 | kinematic_model_ = std::make_shared(); 47 | break; 48 | case Biped: 49 | dynamic_model_ = std::make_shared(); 50 | kinematic_model_ = std::make_shared(); 51 | break; 52 | case Hyq: 53 | dynamic_model_ = std::make_shared(); 54 | kinematic_model_ = std::make_shared(); 55 | break; 56 | case Anymal: 57 | dynamic_model_ = std::make_shared(); 58 | kinematic_model_ = std::make_shared(); 59 | break; 60 | case Champ: 61 | dynamic_model_ = std::make_shared(); 62 | kinematic_model_ = std::make_shared(); 63 | break; 64 | default: 65 | assert(false); // Error: Robot model not implemented. 66 | break; 67 | } 68 | } 69 | 70 | 71 | } // namespace towr 72 | 73 | 74 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/spline_acc_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef SPLINE_ACC_CONSTRAINT_H_ 31 | #define SPLINE_ACC_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief Ensures continuous accelerations between polynomials. 41 | * 42 | * This is used to restrict jumps in linear and angular base accelerations, 43 | * since this would require jumps in foot positions or endeffector forces, 44 | * which aren't allowed in our formulation. 45 | * 46 | * @ingroup Constraints 47 | */ 48 | class SplineAccConstraint : public ifopt::ConstraintSet { 49 | public: 50 | SplineAccConstraint(const NodeSpline::Ptr& spline, std::string name); 51 | virtual ~SplineAccConstraint() = default; 52 | 53 | VectorXd GetValues() const override; 54 | VecBound GetBounds() const override; 55 | void FillJacobianBlock (std::string var_set, Jacobian&) const override; 56 | 57 | private: 58 | NodeSpline::Ptr spline_; ///< a spline comprised of polynomials 59 | std::string node_variables_id_; /// polynomial parameterized node values 60 | 61 | int n_junctions_; ///< number of junctions between polynomials in spline. 62 | int n_dim_; ///< dimensions that this polynomial represents (e.g. x,y). 63 | std::vector T_; ///< Duration of each polynomial in spline. 64 | }; 65 | 66 | } /* namespace towr */ 67 | 68 | #endif /* SPLINE_ACC_CONSTRAINT_H_ */ 69 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/total_duration_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_TOTAL_DURATION_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_TOTAL_DURATION_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief Makes sure all the phase durations sum up to the total time. 41 | * 42 | * When optimizing over the phase durations of each foot, this constraint 43 | * makes sure that: 44 | * t_phase_1 + ... + t_phase_(n-1) = T_total. 45 | * 46 | * Attention: At this point last phase duration is not an optimization variable 47 | * and a way should be found to optimize over all phases while setting the 48 | * total duration by constraint and not through hard parameterization. 49 | * 50 | * @ingroup Constraints 51 | */ 52 | class TotalDurationConstraint : public ifopt::ConstraintSet { 53 | public: 54 | using EE = uint; 55 | 56 | TotalDurationConstraint(double T_total, int ee); 57 | ~TotalDurationConstraint() = default; 58 | 59 | void InitVariableDependedQuantities(const VariablesPtr& x) override; 60 | 61 | VectorXd GetValues() const override; 62 | VecBound GetBounds() const override; 63 | void FillJacobianBlock (std::string var_set, Jacobian&) const override; 64 | 65 | private: 66 | PhaseDurations::Ptr phase_durations_; 67 | double T_total_; 68 | EE ee_; 69 | }; 70 | 71 | } // namespace towr 72 | 73 | #endif /* TOWR_CONSTRAINTS_TOTAL_DURATION_CONSTRAINT_H_ */ 74 | -------------------------------------------------------------------------------- /towr/include/towr/models/examples/anymal_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ 31 | #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief The Kinematics of the quadruped robot ANYmal. 41 | */ 42 | class AnymalKinematicModel : public KinematicModel { 43 | public: 44 | AnymalKinematicModel () : KinematicModel(4) 45 | { 46 | const double x_nominal_b = 0.34; 47 | const double y_nominal_b = 0.19; 48 | const double z_nominal_b = -0.42; 49 | 50 | nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; 51 | nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; 52 | nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; 53 | nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; 54 | 55 | max_dev_from_nominal_ << 0.15, 0.1, 0.10; 56 | } 57 | }; 58 | 59 | /** 60 | * @brief The Dynamics of the quadruped robot ANYmal. 61 | */ 62 | class AnymalDynamicModel : public SingleRigidBodyDynamics { 63 | public: 64 | AnymalDynamicModel() 65 | : SingleRigidBodyDynamics(29.5, 66 | 0.946438, 1.94478, 2.01835, 0.000938112, -0.00595386, -0.00146328, 67 | 4) {} 68 | }; 69 | 70 | } // namespace towr 71 | 72 | #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ */ 73 | -------------------------------------------------------------------------------- /towr/include/towr/variables/phase_durations_observer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_DURATIONS_OBSERVER_H_ 31 | #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_DURATIONS_OBSERVER_H_ 32 | 33 | 34 | namespace towr { 35 | 36 | class PhaseDurations; 37 | 38 | /** 39 | * @brief Base class to receive up-to-date values of the ContactSchedule. 40 | * 41 | * This class registers with the contact schedule and everytime those 42 | * durations change, the contact schedule updates this class by calling the 43 | * UpdatePhaseDurations() method. 44 | * 45 | * Used by spline.h 46 | * 47 | * This class implements the observer pattern: 48 | * https://sourcemaking.com/design_patterns/observer 49 | */ 50 | class PhaseDurationsObserver { 51 | public: 52 | using PhaseDurationsSubjectPtr = PhaseDurations*; // observer shouldn't own subject 53 | 54 | PhaseDurationsObserver() = default; 55 | 56 | /** 57 | * @brief Registers this observer with the subject class to receive updates. 58 | * @param phase_durations A pointer to the hase durations subject. 59 | */ 60 | PhaseDurationsObserver(PhaseDurationsSubjectPtr phase_durations); 61 | virtual ~PhaseDurationsObserver() = default; 62 | 63 | /** 64 | * @brief Callback method called every time the subject changes. 65 | */ 66 | virtual void UpdatePolynomialDurations() = 0; 67 | 68 | protected: 69 | PhaseDurationsSubjectPtr phase_durations_; 70 | }; 71 | 72 | } /* namespace towr */ 73 | 74 | #endif /* TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_DURATIONS_OBSERVER_H_ */ 75 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, nationality, personal appearance, race, religion, or sexual identity and orientation. 6 | 7 | ## Our Standards 8 | 9 | Examples of behavior that contributes to creating a positive environment include: 10 | 11 | * Using welcoming and inclusive language 12 | * Being respectful of differing viewpoints and experiences 13 | * Gracefully accepting constructive criticism 14 | * Focusing on what is best for the community 15 | * Showing empathy towards other community members 16 | 17 | Examples of unacceptable behavior by participants include: 18 | 19 | * The use of sexualized language or imagery and unwelcome sexual attention or advances 20 | * Trolling, insulting/derogatory comments, and personal or political attacks 21 | * Public or private harassment 22 | * Publishing others' private information, such as a physical or electronic address, without explicit permission 23 | * Other conduct which could reasonably be considered inappropriate in a professional setting 24 | 25 | ## Our Responsibilities 26 | 27 | Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior. 28 | 29 | Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful. 30 | 31 | ## Scope 32 | 33 | This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers. 34 | 35 | ## Enforcement 36 | 37 | Instances of abusive, harassing, or otherwise unacceptable behavior may be reported by contacting the project team at alexander.w.winkler@gmail.com. The project team will review and investigate all complaints, and will respond in a way that it deems appropriate to the circumstances. The project team is obligated to maintain confidentiality with regard to the reporter of an incident. Further details of specific enforcement policies may be posted separately. 38 | 39 | Project maintainers who do not follow or enforce the Code of Conduct in good faith may face temporary or permanent repercussions as determined by other members of the project's leadership. 40 | 41 | ## Attribution 42 | 43 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at [http://contributor-covenant.org/version/1/4][version] 44 | 45 | [homepage]: http://contributor-covenant.org 46 | [version]: http://contributor-covenant.org/version/1/4/ 47 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/swing_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_SWING_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_SWING_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief Constrains the foot position during the swing-phase. 41 | * 42 | * This avoids very quick swinging of the feet, where the polynomial then 43 | * leaves the e.g. range-of-motion in between nodes. This constraint can also 44 | * be used to force a leg lift. However, it is cleanest if the optimization 45 | * can be performed without this heuristic constraint. 46 | * 47 | * @ingroup Constraints 48 | */ 49 | class SwingConstraint : public ifopt::ConstraintSet { 50 | public: 51 | using Vector2d = Eigen::Vector2d; 52 | 53 | /** 54 | * @brief Links the swing constraint with current foot variables. 55 | * @param ee_motion_id The name of the foot variables in the optimization. 56 | */ 57 | SwingConstraint (std::string ee_motion_id); 58 | virtual ~SwingConstraint () = default; 59 | 60 | VectorXd GetValues() const override; 61 | VecBound GetBounds() const override; 62 | void FillJacobianBlock (std::string var_set, Jacobian&) const override; 63 | 64 | void InitVariableDependedQuantities(const VariablesPtr& x) override; 65 | 66 | private: 67 | NodesVariablesPhaseBased::Ptr ee_motion_; 68 | double t_swing_avg_ = 0.3; 69 | std::string ee_motion_id_; 70 | 71 | std::vector pure_swing_node_ids_; 72 | }; 73 | 74 | } /* namespace towr */ 75 | 76 | #endif /* TOWR_CONSTRAINTS_SWING_CONSTRAINT_H_ */ 77 | -------------------------------------------------------------------------------- /towr_ros/include/towr_ros/towr_user_interface.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_ROS_USER_INTERFACE_H_ 31 | #define TOWR_ROS_USER_INTERFACE_H_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief Translates user input into the ROS message TowrCommand.msg. 41 | * 42 | * This includes high level input about where to go (e.g. converting 43 | * keyboard input into a goal state), which terrain to visualize, etc. 44 | */ 45 | class TowrUserInterface { 46 | public: 47 | /** 48 | * @brief Constructs default object to interact with framework. 49 | */ 50 | TowrUserInterface (); 51 | virtual ~TowrUserInterface () = default; 52 | 53 | /** 54 | * Called whenever a keyboard key is pressed. 55 | * @param c Unicode character of that key (see ncurses library). 56 | */ 57 | void CallbackKey(int c); 58 | 59 | private: 60 | ::ros::Publisher user_command_pub_; ///< the output message to TOWR. 61 | 62 | 63 | void PublishCommand(); 64 | 65 | xpp::State3dEuler goal_geom_; 66 | int terrain_; 67 | int gait_combo_; 68 | int robot_; 69 | bool visualize_trajectory_; 70 | bool play_initialization_; 71 | double replay_speed_; 72 | bool plot_trajectory_; 73 | bool optimize_; 74 | bool publish_optimized_trajectory_; 75 | double total_duration_; 76 | bool optimize_phase_durations_; 77 | 78 | int AdvanceCircularBuffer(int& curr, int max) const; 79 | 80 | void PrintVector(const Eigen::Vector3d& v) const; 81 | void PrintVector2D(const Eigen::Vector2d& v) const; 82 | void PrintScreen() const; 83 | }; 84 | 85 | } /* namespace towr */ 86 | 87 | #endif /* TOWR_ROS_USER_INTERFACE_H_ */ 88 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/base_motion_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_BASE_MOTION_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_BASE_MOTION_CONSTRAINT_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include "time_discretization_constraint.h" 37 | 38 | namespace towr { 39 | 40 | /** 41 | * @brief Keeps the 6D base motion in a specified range. 42 | * 43 | * In general this constraint should be avoided, since a similar affect can be 44 | * achieved with RangeOfMotionConstraint. 45 | * 46 | * @ingroup Constraints 47 | */ 48 | class BaseMotionConstraint : public TimeDiscretizationConstraint { 49 | public: 50 | /** 51 | * @brief Links the base variables and sets hardcoded bounds on the state. 52 | * @param T The total time of the optimization horizon. 53 | * @param dt The discretization interval of the constraints. 54 | * @param spline_holder Holds pointers to the base variables. 55 | */ 56 | BaseMotionConstraint (double T, double dt, const SplineHolder& spline_holder); 57 | virtual ~BaseMotionConstraint () = default; 58 | 59 | void UpdateConstraintAtInstance (double t, int k, VectorXd& g) const override; 60 | void UpdateBoundsAtInstance (double t, int k, VecBound&) const override; 61 | void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; 62 | 63 | private: 64 | NodeSpline::Ptr base_linear_; 65 | NodeSpline::Ptr base_angular_; 66 | 67 | VecBound node_bounds_; ///< same bounds for each discretized node 68 | int GetRow (int node, int dim) const; 69 | }; 70 | 71 | } /* namespace towr */ 72 | 73 | #endif /* TOWR_CONSTRAINTS_BASE_MOTION_CONSTRAINT_H_ */ 74 | -------------------------------------------------------------------------------- /towr_ros/src/goal_pose_publisher.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include // listen to goal state 37 | #include 38 | 39 | 40 | namespace towr { 41 | 42 | static ros::Publisher rviz_pub; 43 | 44 | void UserCommandCallback(const towr_ros::TowrCommand& msg_in) 45 | { 46 | // get which terrain 47 | auto terrain_id = static_cast(msg_in.terrain); 48 | auto terrain_ = HeightMap::MakeTerrain(terrain_id); 49 | 50 | geometry_msgs::PoseStamped goal_msg; 51 | goal_msg.header.frame_id = "world"; 52 | 53 | // visualize goal z state on terrain. 54 | double x = msg_in.goal_lin.pos.x; 55 | double y = msg_in.goal_lin.pos.y; 56 | goal_msg.pose.position.x = x; 57 | goal_msg.pose.position.y = y; 58 | goal_msg.pose.position.z = terrain_->GetHeight(x, y); 59 | 60 | // orientation according to message 61 | Eigen::Quaterniond q = xpp::GetQuaternionFromEulerZYX(msg_in.goal_ang.pos.z, 62 | msg_in.goal_ang.pos.y, 63 | msg_in.goal_ang.pos.x); 64 | goal_msg.pose.orientation = xpp::Convert::ToRos(q); 65 | rviz_pub.publish(goal_msg); 66 | } 67 | 68 | } // namespace towr 69 | 70 | int main(int argc, char *argv[]) 71 | { 72 | ros::init(argc, argv, "goal_pose_publisher"); 73 | 74 | ros::NodeHandle n; 75 | 76 | ros::Subscriber goal_sub; 77 | goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); 78 | towr::rviz_pub = n.advertise("xpp/goal", 1); 79 | 80 | ros::spin(); 81 | 82 | return 1; 83 | } 84 | -------------------------------------------------------------------------------- /towr/src/spline_acc_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace towr { 33 | 34 | SplineAccConstraint::SplineAccConstraint (const NodeSpline::Ptr& spline, 35 | std::string node_variable_name) 36 | :ConstraintSet(kSpecifyLater, "splineacc-" + node_variable_name) 37 | { 38 | spline_ = spline; 39 | node_variables_id_ = node_variable_name; 40 | 41 | n_dim_ = spline->GetPoint(0.0).p().rows(); 42 | n_junctions_ = spline->GetPolynomialCount() - 1; 43 | T_ = spline->GetPolyDurations(); 44 | 45 | SetRows(n_dim_*n_junctions_); 46 | } 47 | 48 | Eigen::VectorXd 49 | SplineAccConstraint::GetValues () const 50 | { 51 | VectorXd g(GetRows()); 52 | 53 | for (int j=0; jGetPoint(p_prev, T_.at(p_prev)).a(); 56 | 57 | int p_next = j+1; 58 | VectorXd acc_next = spline_->GetPoint(p_next, 0.0).a(); 59 | 60 | g.segment(j*n_dim_, n_dim_) = acc_prev - acc_next; 61 | } 62 | 63 | return g; 64 | } 65 | 66 | void 67 | SplineAccConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const 68 | { 69 | if (var_set == node_variables_id_) { 70 | for (int j=0; jGetJacobianWrtNodes(p_prev, T_.at(p_prev), kAcc); 73 | 74 | int p_next = j+1; 75 | Jacobian acc_next = spline_->GetJacobianWrtNodes(p_next, 0.0, kAcc); 76 | 77 | jac.middleRows(j*n_dim_, n_dim_) = acc_prev - acc_next; 78 | } 79 | } 80 | } 81 | 82 | SplineAccConstraint::VecBound 83 | SplineAccConstraint::GetBounds () const 84 | { 85 | return VecBound(GetRows(), ifopt::BoundZero); 86 | } 87 | 88 | } /* namespace xpp */ 89 | 90 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/terrain_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_TERRAIN_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_TERRAIN_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace towr { 39 | 40 | /** 41 | * @brief Ensures the endeffectors always lays on or above terrain height. 42 | * 43 | * When using interior point solvers such as IPOPT to solve the problem, this 44 | * constraint also keeps the foot nodes far from the terrain, causing a leg 45 | * lifting during swing-phase. This is convenient. 46 | * 47 | * Attention: This is enforced only at the spline nodes. 48 | * 49 | * @ingroup Constraints 50 | */ 51 | class TerrainConstraint : public ifopt::ConstraintSet { 52 | public: 53 | using Vector3d = Eigen::Vector3d; 54 | 55 | /** 56 | * @brief Constructs a terrain constraint. 57 | * @param terrain The terrain height value and slope for each position x,y. 58 | * @param ee_motion_id The name of the endeffector variable set. 59 | */ 60 | TerrainConstraint (const HeightMap::Ptr& terrain, std::string ee_motion_id); 61 | virtual ~TerrainConstraint () = default; 62 | 63 | void InitVariableDependedQuantities(const VariablesPtr& x) override; 64 | 65 | VectorXd GetValues() const override; 66 | VecBound GetBounds() const override; 67 | void FillJacobianBlock (std::string var_set, Jacobian&) const override; 68 | 69 | private: 70 | NodesVariablesPhaseBased::Ptr ee_motion_; ///< the position of the endeffector. 71 | HeightMap::Ptr terrain_; ///< the height map of the current terrain. 72 | 73 | std::string ee_motion_id_; ///< the name of the endeffector variable set. 74 | std::vector node_ids_; ///< the indices of the nodes constrained. 75 | }; 76 | 77 | } /* namespace towr */ 78 | 79 | #endif /* TOWR_CONSTRAINTS_TERRAIN_CONSTRAINT_H_ */ 80 | -------------------------------------------------------------------------------- /towr/src/spline_holder.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | namespace towr{ 34 | 35 | SplineHolder::SplineHolder (NodesVariables::Ptr base_lin_nodes, 36 | NodesVariables::Ptr base_ang_nodes, 37 | const std::vector& base_poly_durations, 38 | std::vector ee_motion_nodes, 39 | std::vector ee_force_nodes, 40 | std::vector phase_durations, 41 | bool durations_change) 42 | { 43 | base_linear_ = std::make_shared(base_lin_nodes.get(), base_poly_durations); 44 | base_angular_ = std::make_shared(base_ang_nodes.get(), base_poly_durations); 45 | phase_durations_ = phase_durations; 46 | 47 | for (uint ee=0; ee(ee_motion_nodes.at(ee), phase_durations.at(ee).get())); 51 | ee_force_.push_back(std::make_shared(ee_force_nodes.at(ee), phase_durations.at(ee).get())); 52 | } else { 53 | // spline without changing the polynomial durations 54 | auto ee_motion_poly_durations = ee_motion_nodes.at(ee)->ConvertPhaseToPolyDurations(phase_durations.at(ee)->GetPhaseDurations()); 55 | auto ee_force_poly_durations = ee_force_nodes.at(ee)->ConvertPhaseToPolyDurations(phase_durations.at(ee)->GetPhaseDurations()); 56 | 57 | ee_motion_.push_back(std::make_shared(ee_motion_nodes.at(ee).get(), ee_motion_poly_durations)); 58 | ee_force_.push_back (std::make_shared(ee_force_nodes.at(ee).get(), ee_force_poly_durations)); 59 | } 60 | } 61 | } 62 | 63 | } /* namespace towr */ 64 | -------------------------------------------------------------------------------- /towr/src/time_discretization_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace towr { 35 | 36 | 37 | TimeDiscretizationConstraint::TimeDiscretizationConstraint (double T, double dt, 38 | std::string name) 39 | :ConstraintSet(kSpecifyLater, name) 40 | { 41 | double t = 0.0; 42 | dts_ = {t}; 43 | 44 | for (int i=0; i. 69 | // so RF and LH in contact is (Pb): o . 70 | // . o 71 | // flight-phase 72 | ContactState II_; 73 | // 1 swingleg 74 | ContactState PI_; 75 | ContactState bI_; 76 | ContactState IP_; 77 | ContactState Ib_; 78 | // 2 swinglegs 79 | ContactState Pb_; 80 | ContactState bP_; 81 | ContactState BI_; 82 | ContactState IB_; 83 | ContactState PP_; 84 | ContactState bb_; 85 | // 3 swinglegs 86 | ContactState Bb_; 87 | ContactState BP_; 88 | ContactState bB_; 89 | ContactState PB_; 90 | // stance-phase 91 | ContactState BB_; 92 | }; 93 | 94 | } /* namespace towr */ 95 | 96 | #endif /* TOWR_MODELS_QUADRUPED_GAIT_GENERATOR_H_ */ 97 | -------------------------------------------------------------------------------- /towr/include/towr/models/kinematic_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_MODELS_KINEMATIC_MODEL_H_ 31 | #define TOWR_MODELS_KINEMATIC_MODEL_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace towr { 39 | 40 | /** 41 | * @brief Contains all the robot specific kinematic parameters. 42 | * 43 | * This class is mainly used to formulate the @ref RangeOfMotionConstraint, 44 | * restricting each endeffector to stay inside it's kinematic range. 45 | * 46 | * @ingroup Robots 47 | */ 48 | class KinematicModel { 49 | public: 50 | using Ptr = std::shared_ptr; 51 | using EEPos = std::vector; 52 | using Vector3d = Eigen::Vector3d; 53 | 54 | /** 55 | * @brief Constructs a kinematic model of a robot with zero range of motion. 56 | * @param n_ee The number of endeffectors of the robot. 57 | */ 58 | KinematicModel (int n_ee) 59 | { 60 | nominal_stance_.resize(n_ee); 61 | max_dev_from_nominal_.setZero(); 62 | } 63 | 64 | virtual ~KinematicModel () = default; 65 | 66 | /** 67 | * @brief The xyz-position [m] of each foot in default stance. 68 | * @returns The vector from base to each foot expressed in the base frame. 69 | */ 70 | virtual EEPos GetNominalStanceInBase() const 71 | { 72 | return nominal_stance_; 73 | } 74 | 75 | /** 76 | * @brief How far each foot can deviate from its nominal position. 77 | * @return The deviation [m] expresed in the base frame. 78 | */ 79 | virtual Vector3d GetMaximumDeviationFromNominal() const 80 | { 81 | return max_dev_from_nominal_; 82 | } 83 | 84 | /** 85 | * @returns returns the number of endeffectors of this robot. 86 | */ 87 | int GetNumberOfEndeffectors() const 88 | { 89 | return nominal_stance_.size(); 90 | } 91 | 92 | protected: 93 | EEPos nominal_stance_; 94 | Vector3d max_dev_from_nominal_; 95 | }; 96 | 97 | } /* namespace towr */ 98 | 99 | #endif /* TOWR_MODELS_KINEMATIC_MODEL_H_ */ 100 | -------------------------------------------------------------------------------- /towr/include/towr/variables/spline_holder.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_SPLINE_HOLDER_H_ 31 | #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_SPLINE_HOLDER_H_ 32 | 33 | #include "phase_durations.h" 34 | #include "node_spline.h" 35 | #include "nodes_variables.h" 36 | #include "nodes_variables_phase_based.h" 37 | 38 | namespace towr { 39 | 40 | /** 41 | * @brief Builds splines from node values (pos/vel) and durations. 42 | * 43 | * These splines are linked to the optimization variables, so change as the 44 | * nodes or durations change. This is a convenience class to not have 45 | * to construct the splines from the variables new every time. 46 | */ 47 | struct SplineHolder { 48 | /** 49 | * @brief Fully construct all splines. 50 | * @param base_lin The nodes describing the base linear motion. 51 | * @param base_ang The nodes describing the base angular motion. 52 | * @param base_poly_durations The durations of each base polynomial. 53 | * @param ee_motion The nodes describing the endeffector motions. 54 | * @param ee_force The nodes describing the endeffector forces. 55 | * @param phase_durations The phase durations of each endeffector. 56 | * @param ee_durations_change True if the ee durations are optimized over. 57 | */ 58 | SplineHolder (NodesVariables::Ptr base_lin, 59 | NodesVariables::Ptr base_ang, 60 | const std::vector& base_poly_durations, 61 | std::vector ee_motion, 62 | std::vector ee_force, 63 | std::vector phase_durations, 64 | bool ee_durations_change); 65 | 66 | /** 67 | * @brief Attention, nothing initialized. 68 | */ 69 | SplineHolder () = default; 70 | 71 | NodeSpline::Ptr base_linear_; 72 | NodeSpline::Ptr base_angular_; 73 | 74 | std::vector ee_motion_; 75 | std::vector ee_force_; 76 | std::vector phase_durations_; 77 | }; 78 | 79 | } /* namespace towr */ 80 | 81 | #endif /* TOWR_TOWR_INCLUDE_TOWR_VARIABLES_SPLINE_HOLDER_H_ */ 82 | -------------------------------------------------------------------------------- /towr_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(towr_ros) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rosbag 9 | std_msgs 10 | visualization_msgs 11 | message_generation 12 | xpp_msgs 13 | xpp_states 14 | towr 15 | ) 16 | 17 | 18 | ## Generate messages in the 'msg' folder 19 | add_message_files( FILES TowrCommand.msg ) 20 | generate_messages( DEPENDENCIES std_msgs xpp_msgs ) 21 | 22 | 23 | ################################### 24 | ## catkin specific configuration ## 25 | ################################### 26 | ## The catkin_package macro generates cmake config files for your package 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES ${PROJECT_NAME} 30 | CATKIN_DEPENDS xpp_states roscpp 31 | DEPENDS towr 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | # The interface to derive from for ros integration 43 | add_library(${PROJECT_NAME} SHARED 44 | src/towr_ros_interface.cc 45 | ) 46 | add_dependencies(${PROJECT_NAME} 47 | ${PROJECT_NAME}_gencpp 48 | ) 49 | target_link_libraries(${PROJECT_NAME} 50 | ${catkin_LIBRARIES} 51 | ) 52 | 53 | 54 | ## An example executable that runs TOWR 55 | add_executable(towr_ros_app 56 | src/towr_ros_app.cc 57 | ) 58 | add_dependencies(towr_ros_app 59 | ${PROJECT_NAME}_gencpp 60 | ) 61 | target_link_libraries(towr_ros_app 62 | ${catkin_LIBRARIES} 63 | ${PROJECT_NAME} 64 | ) 65 | 66 | 67 | ## Keyboard interfaces (ncurses) 68 | find_package(Curses REQUIRED) 69 | add_executable(towr_user_interface 70 | src/towr_user_interface.cc 71 | ) 72 | add_dependencies(towr_user_interface 73 | ${PROJECT_NAME}_gencpp 74 | ) 75 | target_link_libraries(towr_user_interface 76 | ${catkin_LIBRARIES} 77 | ${CURSES_LIBRARIES} 78 | ) 79 | 80 | 81 | # Display different terrains in rviz 82 | add_executable(rviz_terrain_publisher 83 | src/rviz_terrain_publisher.cc 84 | ) 85 | add_dependencies(rviz_terrain_publisher 86 | ${PROJECT_NAME}_gencpp 87 | ) 88 | target_link_libraries(rviz_terrain_publisher 89 | ${catkin_LIBRARIES} 90 | ) 91 | 92 | 93 | # Display goal pose in rviz 94 | add_executable(goal_pose_publisher 95 | src/goal_pose_publisher.cc 96 | ) 97 | add_dependencies(goal_pose_publisher 98 | ${PROJECT_NAME}_gencpp 99 | ) 100 | target_link_libraries(goal_pose_publisher 101 | ${catkin_LIBRARIES} 102 | ) 103 | 104 | 105 | # (Optional) to manipulate ROS bags 106 | add_executable(rosbag_traj_combiner 107 | src/rosbag_traj_combiner.cc 108 | ) 109 | target_link_libraries(rosbag_traj_combiner 110 | ${catkin_LIBRARIES} 111 | ) 112 | 113 | 114 | add_executable(rosbag_geom_msg_extractor 115 | src/rosbag_geom_msg_extractor.cc 116 | ) 117 | target_link_libraries(rosbag_geom_msg_extractor 118 | ${catkin_LIBRARIES} 119 | ) 120 | 121 | 122 | ############# 123 | ## Install ## 124 | ############# 125 | # Mark library for installation 126 | install( 127 | TARGETS towr_ros_app 128 | towr_user_interface 129 | rviz_terrain_publisher 130 | goal_pose_publisher 131 | rosbag_traj_combiner 132 | rosbag_geom_msg_extractor 133 | ${PROJECT_NAME} 134 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 135 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 137 | ) 138 | 139 | # Mark header files for installation 140 | install( 141 | DIRECTORY include/${PROJECT_NAME}/ 142 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 143 | FILES_MATCHING PATTERN "*.h" 144 | ) 145 | 146 | # Mark other files for installation 147 | install( 148 | DIRECTORY launch rviz bash 149 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 150 | ) 151 | -------------------------------------------------------------------------------- /towr_ros/src/rosbag_geom_msg_extractor.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | /** 42 | * Extracts the standard ROS geometry_msgs/Vector3.h from a ROS bag of 43 | * RobotStateCartesian and writes them to a new bag. The bags with standard 44 | * messages can then easily be imported and plotted in matlab. 45 | * 46 | * See towr/matlab/plot_rosbag.m for an example of how to open these. 47 | */ 48 | int main(int argc, char *argv[]) 49 | { 50 | if (argc==1) { 51 | std::cerr << "Error: Please enter path to bag file\n"; 52 | return 0; 53 | } 54 | 55 | std::string bag_file = argv[1]; 56 | 57 | rosbag::Bag bag_r; 58 | bag_r.open(bag_file, rosbag::bagmode::Read); 59 | std::cout << "Reading from bag " + bag_r.getFileName() << std::endl; 60 | 61 | // select which iterations (message topics) to be included in bag file 62 | std::string topic = "/xpp/state_des"; 63 | rosbag::View view(bag_r, rosbag::TopicQuery(topic)); 64 | if (view.size() == 0) { 65 | std::cerr << "Error: Topic " << topic << " doesn't exist\n"; 66 | return 0; 67 | } 68 | 69 | // write the message with modified timestamp into new bag file 70 | rosbag::Bag bag_w; 71 | bag_w.open("/home/winklera/Desktop/matlab_rdy.bag", rosbag::bagmode::Write); 72 | 73 | BOOST_FOREACH(rosbag::MessageInstance const m, view) 74 | { 75 | ros::Time t = m.getTime(); 76 | auto state_msg = m.instantiate(); 77 | bag_w.write("base_pose", t, state_msg->base.pose); 78 | bag_w.write("base_acc", t, state_msg->base.accel.linear); 79 | 80 | int n_feet = state_msg->ee_motion.size(); 81 | 82 | for (int i=0; iee_motion.at(i).pos); 84 | bag_w.write("foot_force_"+std::to_string(i), t, state_msg->ee_forces.at(i)); 85 | } 86 | } 87 | 88 | bag_r.close(); 89 | std::cout << "Successfully created bag " + bag_w.getFileName() << std::endl; 90 | bag_w.close(); 91 | } 92 | -------------------------------------------------------------------------------- /towr/include/towr/costs/soft_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_COSTS_SOFT_CONSTRAINT_H_ 31 | #define TOWR_COSTS_SOFT_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | namespace towr { 36 | 37 | /** 38 | * @brief Converts a constraint to a cost by weighing the quadratic violations. 39 | * 40 | * Let constraint g(x) \in R^m with upper bound b_u and lower bound b_l. 41 | * Let 42 | * 43 | * g'(x) = g(x) - 0.5(b_u+b_l) = g(x) - b 44 | * 45 | * And it's derivative 46 | * 47 | * dg'(x)/dx = J(x). 48 | * 49 | * Define a cost as 50 | * 51 | * c(x) = 0.5 * g'^T * W * g', where W = diag(w1,...,wm). 52 | * 53 | * Then the gradient of the cost is defined as: 54 | * 55 | * dc(x)/dx = (g'(x)^T * W * J)^T = J^T * W * (g(x)-b). 56 | * 57 | * @ingroup Costs 58 | */ 59 | class SoftConstraint : public ifopt::Component { 60 | public: 61 | using ConstraintPtr = Component::Ptr; 62 | 63 | /** 64 | * @brief Creates a soft constraint (=cost) from a hard constraint. 65 | * @param constraint The fully initialized constraint. 66 | * 67 | * Weights are set to identity, so each constraint violation contributes 68 | * equally to the cost. 69 | */ 70 | SoftConstraint (const ConstraintPtr& constraint); 71 | virtual ~SoftConstraint () = default; 72 | 73 | private: 74 | ConstraintPtr constraint_; 75 | VectorXd W_; ///< weights how each constraint violation contributes to the cost. 76 | VectorXd b_; /// average value of each upper and lower bound. 77 | 78 | /** 79 | * @brief The quadratic constraint violation transformed to a cost. 80 | * 81 | * c(x) = 0.5 * (g-b)^T * W * (g-b) 82 | */ 83 | VectorXd GetValues () const override; 84 | 85 | /** 86 | * @brief The row-vector of derivatives of the cost term. 87 | * 88 | * dc(x)/dx = J^T * W * (g-b) 89 | */ 90 | Jacobian GetJacobian() const override; 91 | 92 | // doesn't exist for cost, generated run-time error when used 93 | VecBound GetBounds() const final { return VecBound(GetRows(), ifopt::NoBound); }; 94 | void SetVariables(const VectorXd& x) final { assert(false); }; 95 | }; 96 | 97 | } /* namespace towr */ 98 | 99 | #endif /* TOWR_COSTS_SOFT_CONSTRAINT_H_ */ 100 | -------------------------------------------------------------------------------- /towr/include/towr/models/robot_model.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_MODELS_ROBOT_MODEL_H_ 31 | #define TOWR_MODELS_ROBOT_MODEL_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | namespace towr { 40 | 41 | /** 42 | * @defgroup Robots 43 | * @brief The kinematic and dynamic model of the robot. 44 | * 45 | * These models contain all the robot specific quantities in this problem. 46 | * 47 | * ### Add your own robot 48 | * To add your own robot, you must create its KinematicModel and DynamicModel. 49 | * The kinematics simply define a workspace for each end-effector. The 50 | * Dynamic Model can be everything from a Linear Inverted Pendulum, 51 | * SingleRigidBodyDynamics (SRBD), Centroidal Dynamics to Full-Rigid-Body 52 | * Dynamics (RBD). This library provides an implementation for the 53 | * SingleRigidBodyDynamics in which only the combined mass and inertia must 54 | * be adapted, but other models can be used as well. For example robots 55 | * to use as a guideline, see \ref include/towr/models/examples. 56 | */ 57 | 58 | /** 59 | * @brief Base class for robot specific kinematics and dynamics. 60 | * 61 | * @ingroup Robots 62 | */ 63 | struct RobotModel { 64 | /** 65 | * @brief Robots for which kinematic and dynamic models are implemented. 66 | * 67 | * See folder: \ref include/towr/models/examples for more information. 68 | * @ingroup Robots 69 | */ 70 | enum Robot { Monoped, ///< one-legged hopper 71 | Biped, ///< two-legged 72 | Hyq, ///< four-legged robot from IIT 73 | Anymal, ///< four-legged robot from Anybotics 74 | Champ, ///< four-legged robot 75 | ROBOT_COUNT }; 76 | 77 | 78 | RobotModel() = default; 79 | RobotModel(Robot robot); 80 | 81 | KinematicModel::Ptr kinematic_model_; 82 | DynamicModel::Ptr dynamic_model_; 83 | }; 84 | 85 | 86 | const static std::map robot_names = 87 | { 88 | {RobotModel::Monoped, "Monoped"}, 89 | {RobotModel::Biped, "Biped"}, 90 | {RobotModel::Hyq, "Hyq"}, 91 | {RobotModel::Anymal, "Anymal"}, 92 | {RobotModel::Champ, "Champ"} 93 | }; 94 | 95 | } /* namespace towr */ 96 | 97 | #endif /* TOWR_MODELS_ROBOT_MODEL_H_ */ 98 | -------------------------------------------------------------------------------- /towr/src/monoped_gait_generator.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace towr { 36 | 37 | void 38 | MonopedGaitGenerator::SetCombo (Combos combo) 39 | { 40 | switch (combo) { 41 | case C0: SetGaits({Stand, Hop1, Hop1, Hop1, Hop1, Stand}); break; 42 | case C1: SetGaits({Stand, Hop1, Hop1, Hop1, Stand}); break; 43 | case C2: SetGaits({Stand, Hop1, Hop1, Hop1, Hop1, Stand}); break; 44 | case C3: SetGaits({Stand, Hop2, Hop2, Hop2, Stand}); break; 45 | case C4: SetGaits({Stand, Hop2, Hop2, Hop2, Hop2, Hop2, Stand}); break; 46 | default: assert(false); std::cout << "Gait not defined\n"; break; 47 | } 48 | } 49 | 50 | MonopedGaitGenerator::GaitInfo 51 | MonopedGaitGenerator::GetGait (Gaits gait) const 52 | { 53 | switch (gait) { 54 | case Stand: return GetStrideStand(); 55 | case Flight: return GetStrideFlight(); 56 | case Hop1: return GetStrideHop(); 57 | case Hop2: return GetStrideHopLong(); 58 | default: assert(false); // gait not implemented 59 | } 60 | } 61 | 62 | MonopedGaitGenerator::GaitInfo 63 | MonopedGaitGenerator::GetStrideStand () const 64 | { 65 | auto times = 66 | { 67 | 0.5, 68 | }; 69 | auto contacts = 70 | { 71 | o_, 72 | }; 73 | 74 | return std::make_pair(times, contacts); 75 | } 76 | 77 | MonopedGaitGenerator::GaitInfo 78 | MonopedGaitGenerator::GetStrideFlight () const 79 | { 80 | auto times = 81 | { 82 | 0.5, 83 | }; 84 | auto contacts = 85 | { 86 | x_, 87 | }; 88 | 89 | return std::make_pair(times, contacts); 90 | } 91 | 92 | MonopedGaitGenerator::GaitInfo 93 | MonopedGaitGenerator::GetStrideHop () const 94 | { 95 | auto times = 96 | { 97 | 0.3, 0.3, 98 | }; 99 | auto contacts = 100 | { 101 | o_, x_, 102 | }; 103 | 104 | return std::make_pair(times, contacts); 105 | } 106 | 107 | MonopedGaitGenerator::GaitInfo 108 | MonopedGaitGenerator::GetStrideHopLong () const 109 | { 110 | auto times = 111 | { 112 | 0.2, 0.3, 113 | }; 114 | auto contacts = 115 | { 116 | o_, x_, 117 | }; 118 | 119 | return std::make_pair(times, contacts); 120 | } 121 | 122 | } /* namespace towr */ 123 | -------------------------------------------------------------------------------- /towr/include/towr/variables/phase_spline.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_SPLINE_H_ 31 | #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_SPLINE_H_ 32 | 33 | #include "node_spline.h" 34 | #include "phase_durations_observer.h" 35 | #include "nodes_variables_phase_based.h" 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief A spline built from node values and polynomial durations. 41 | * 42 | * This class is responsible for combining the optimized node values with 43 | * the optimized phase durations to construct a sequence of 44 | * CubicHermitePolynomial. For this it observers whether one of the quantities 45 | * changed and then updates all the polynomials accordingly. 46 | */ 47 | class PhaseSpline : public NodeSpline, public PhaseDurationsObserver{ 48 | public: 49 | using Ptr = std::shared_ptr; 50 | using VectorXd = Eigen::VectorXd; 51 | 52 | /** 53 | * @brief Constructs a spline with varying/optimized phase durations. 54 | * @param node_variables The optimized node variables (pos, vel). 55 | * @param phase_durations Pointer to the changing phase duration variables. 56 | */ 57 | PhaseSpline(NodesVariablesPhaseBased::Ptr const node_variables, 58 | PhaseDurations* const phase_durations); 59 | ~PhaseSpline() = default; 60 | 61 | /** 62 | * @brief Called by subject to update polynomial durations when they changed. 63 | */ 64 | void UpdatePolynomialDurations() override; 65 | 66 | /** 67 | * @brief How the spline position changes when the polynomial durations change. 68 | * @param t The time along the spline at which the sensitivity is required. 69 | * @return the pxn Jacobian, where: 70 | * p: Number of dimensions of the spline 71 | * n: Number of optimized durations. 72 | */ 73 | Jacobian GetJacobianOfPosWrtDurations(double t) const override; 74 | 75 | private: 76 | /** 77 | * @brief How the position at time t changes with current phase duration. 78 | * @param t The global time along the spline. 79 | * @return How a duration change affects the x,y,z position. 80 | */ 81 | Eigen::VectorXd GetDerivativeOfPosWrtPhaseDuration (double t) const; 82 | 83 | NodesVariablesPhaseBased::Ptr phase_nodes_; // retain pointer for extended functionality 84 | }; 85 | 86 | } /* namespace towr */ 87 | 88 | #endif /* TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_SPLINE_H_ */ 89 | -------------------------------------------------------------------------------- /towr/src/spline.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include // std::accumulate 33 | 34 | namespace towr { 35 | 36 | Spline::Spline(const VecTimes& poly_durations, int n_dim) 37 | { 38 | uint n_polys = poly_durations.size(); 39 | 40 | cubic_polys_.assign(n_polys, CubicHermitePolynomial(n_dim)); 41 | for (int i=0; i= 0.0); 53 | 54 | double t = 0; 55 | int i=0; 56 | for (double d: durations) { 57 | t += d; 58 | 59 | if (t >= t_global-eps) // at junctions, returns previous spline (=) 60 | return i; 61 | 62 | i++; 63 | } 64 | 65 | assert(false); // this should never be reached 66 | } 67 | 68 | std::pair 69 | Spline::GetLocalTime (double t_global, const VecTimes& durations) const 70 | { 71 | int id = GetSegmentID(t_global, durations); 72 | 73 | double t_local = t_global; 74 | for (int i=0; i 31 | #include 32 | #include 33 | #include 34 | 35 | namespace towr { 36 | 37 | 38 | BaseMotionConstraint::BaseMotionConstraint (double T, double dt, 39 | const SplineHolder& spline_holder) 40 | :TimeDiscretizationConstraint(T, dt, "baseMotion") 41 | { 42 | base_linear_ = spline_holder.base_linear_; 43 | base_angular_ = spline_holder.base_angular_; 44 | 45 | double dev_rad = 0.05; 46 | node_bounds_.resize(k6D); 47 | node_bounds_.at(AX) = Bounds(-dev_rad, dev_rad); 48 | node_bounds_.at(AY) = Bounds(-dev_rad, dev_rad); 49 | node_bounds_.at(AZ) = ifopt::NoBound;//Bounds(-dev_rad, dev_rad); 50 | 51 | double z_init = base_linear_->GetPoint(0.0).p().z(); 52 | node_bounds_.at(LX) = ifopt::NoBound; 53 | node_bounds_.at(LY) = ifopt::NoBound;//Bounds(-0.05, 0.05); 54 | node_bounds_.at(LZ) = Bounds(z_init-0.02, z_init+0.1); // allow to move dev_z cm up and down 55 | 56 | int n_constraints_per_node = node_bounds_.size(); 57 | SetRows(GetNumberOfNodes()*n_constraints_per_node); 58 | } 59 | 60 | void 61 | BaseMotionConstraint::UpdateConstraintAtInstance (double t, int k, 62 | VectorXd& g) const 63 | { 64 | g.middleRows(GetRow(k, LX), k3D) = base_linear_->GetPoint(t).p(); 65 | g.middleRows(GetRow(k, AX), k3D) = base_angular_->GetPoint(t).p(); 66 | } 67 | 68 | void 69 | BaseMotionConstraint::UpdateBoundsAtInstance (double t, int k, VecBound& bounds) const 70 | { 71 | for (int dim=0; dimGetJacobianWrtNodes(t, kPos); 82 | 83 | if (var_set == id::base_lin_nodes) 84 | jac.middleRows(GetRow(k,LX), k3D) = base_linear_->GetJacobianWrtNodes(t, kPos); 85 | } 86 | 87 | int 88 | BaseMotionConstraint::GetRow (int node, int dim) const 89 | { 90 | return node*node_bounds_.size() + dim; 91 | } 92 | 93 | } /* namespace towr */ 94 | -------------------------------------------------------------------------------- /towr/src/terrain_constraint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | 33 | namespace towr { 34 | 35 | 36 | TerrainConstraint::TerrainConstraint (const HeightMap::Ptr& terrain, 37 | std::string ee_motion) 38 | :ConstraintSet(kSpecifyLater, "terrain-" + ee_motion) 39 | { 40 | ee_motion_id_ = ee_motion; 41 | terrain_ = terrain; 42 | } 43 | 44 | void 45 | TerrainConstraint::InitVariableDependedQuantities (const VariablesPtr& x) 46 | { 47 | ee_motion_ = x->GetComponent(ee_motion_id_); 48 | 49 | // skip first node, b/c already constrained by initial stance 50 | for (int id=1; idGetNodes().size(); ++id) 51 | node_ids_.push_back(id); 52 | 53 | int constraint_count = node_ids_.size(); 54 | SetRows(constraint_count); 55 | } 56 | 57 | Eigen::VectorXd 58 | TerrainConstraint::GetValues () const 59 | { 60 | VectorXd g(GetRows()); 61 | 62 | auto nodes = ee_motion_->GetNodes(); 63 | int row = 0; 64 | for (int id : node_ids_) { 65 | Vector3d p = nodes.at(id).p(); 66 | g(row++) = p.z() - terrain_->GetHeight(p.x(), p.y()); 67 | } 68 | 69 | return g; 70 | } 71 | 72 | TerrainConstraint::VecBound 73 | TerrainConstraint::GetBounds () const 74 | { 75 | VecBound bounds(GetRows()); 76 | double max_distance_above_terrain = 1e20; // [m] 77 | 78 | int row = 0; 79 | for (int id : node_ids_) { 80 | if (ee_motion_->IsConstantNode(id)) 81 | bounds.at(row) = ifopt::BoundZero; 82 | else 83 | bounds.at(row) = ifopt::Bounds(0.0, max_distance_above_terrain); 84 | row++; 85 | } 86 | 87 | return bounds; 88 | } 89 | 90 | void 91 | TerrainConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const 92 | { 93 | if (var_set == ee_motion_->GetName()) { 94 | auto nodes = ee_motion_->GetNodes(); 95 | int row = 0; 96 | for (int id : node_ids_) { 97 | int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(id, kPos, Z)); 98 | jac.coeffRef(row, idx) = 1.0; 99 | 100 | Vector3d p = nodes.at(id).p(); 101 | for (auto dim : {X,Y}) { 102 | int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(id, kPos, dim)); 103 | jac.coeffRef(row, idx) = -terrain_->GetDerivativeOfHeightWrt(To2D(dim), p.x(), p.y()); 104 | } 105 | row++; 106 | } 107 | } 108 | } 109 | 110 | } /* namespace towr */ 111 | -------------------------------------------------------------------------------- /towr/matlab/plot_rosbag.m: -------------------------------------------------------------------------------- 1 | %% Plot towr rosbags in matlab 2 | % 3 | % The bags can be generated using 4 | % towr_ros/src/exe/rosbag_geom_msg_extractor.cc. 5 | % 6 | % Author: Alexander Winkler 7 | 8 | clc; 9 | clear all; 10 | 11 | %% Extract the desired 3D vectors from the bag 12 | filePath = '~/Desktop/matlab_rdy.bag'; 13 | bag_all = rosbag(filePath); 14 | 15 | t0 = 0.0; %bag_all.StartTime; 16 | T = 3.4; %bag_all.EndTime; 17 | 18 | selectOptions = {'Time', [t0 T] }; 19 | bag = select(bag_all, selectOptions{:}); 20 | 21 | % base motion 22 | bag_base_pose = select(bag, 'Topic', 'base_pose'); 23 | ts_base_pos = timeseries(bag_base_pose, 'Position.X', 'Position.Y', 'Position.Z'); 24 | 25 | bag_base_acc = select(bag, 'Topic', 'base_acc'); 26 | ts_base_acc = timeseries(bag_base_acc, 'Z'); 27 | 28 | % endeffector motion 29 | bag_foot_0 = select(bag, 'Topic', 'foot_pos_0'); 30 | ts_foot_0 = timeseries(bag_foot_0, 'Z'); 31 | 32 | bag_foot_1 = select(bag, 'Topic', 'foot_pos_1'); 33 | ts_foot_1 = timeseries(bag_foot_1, 'Z'); 34 | 35 | % endeffector forces 36 | bag_force_0 = select(bag, 'Topic', 'foot_force_0'); 37 | ts_force_0 = timeseries(bag_force_0, 'Z'); 38 | 39 | bag_force_1 = select(bag, 'Topic', 'foot_force_1'); 40 | ts_force_1 = timeseries(bag_force_1, 'Z'); 41 | 42 | 43 | %% define the plotting range and other additional quantities 44 | t = ts_base_pos.Time; 45 | 46 | % base motion 47 | base_x = ts_base_pos.Data(:,1); 48 | base_y = ts_base_pos.Data(:,2); 49 | base_z = ts_base_pos.Data(:,3); 50 | 51 | % base acceleration 52 | base_zdd = ts_base_acc.Data(:,1); 53 | 54 | % foot motion 55 | foot_0_z = ts_foot_0.Data(:,1); 56 | foot_1_z = ts_foot_1.Data(:,1); 57 | 58 | % foot force 59 | force_0_z = ts_force_0.Data(:,1); 60 | force_1_z = ts_force_1.Data(:,1); 61 | 62 | 63 | % calculate RMSE between base-z acceleration and what should be the 64 | % acceleration based on the forces and gravity. 65 | m = 20; % weight of the robot 66 | g = 9.81; % gravity acceleration 67 | F_ext = force_0_z + force_1_z; 68 | base_zdd_dynamics = 1/m*F_ext - g; 69 | % calculate Root mean square error 70 | base_zdd_error = base_zdd_dynamics - base_zdd; 71 | norm_sqare = norm(base_zdd_error)^2; 72 | n = size(t,1); % number of sampled points 73 | RMSE = sqrt(norm_sqare/n) 74 | 75 | % intervall at which dynamic constraint is enforced 76 | dt_dyn = 0.2; 77 | 78 | spline_durations_f0 = [0.25 0.45 0.34 0.35 0.63 0.13 0.50 0.12 0.56 0.40]; 79 | spline_durations_f1 = [0.62 0.41 0.59 0.14 0.39 0.13 0.78 0.36 0.23 0.46]; 80 | 81 | % create vector with absolute timings 82 | num_phases = size(spline_durations_f0, 2); 83 | dt_foot_0 = spline_durations_f0; 84 | dt_foot_1 = spline_durations_f1; 85 | 86 | for c = 2:num_phases 87 | dt_foot_0(1,c) = dt_foot_0(c-1) + spline_durations_f0(c); 88 | dt_foot_1(1,c) = dt_foot_1(c-1) + spline_durations_f1(c); 89 | end 90 | 91 | 92 | %% plot the values 93 | fh = figure(1); 94 | 95 | sp1 = subplot(3,1,1); 96 | plot(t,base_zdd, 'k'); hold on; 97 | plot(t,base_zdd_dynamics, 'k--'); hold on; 98 | xlim([t0 T]); 99 | sp1.XGrid = 'on'; 100 | sp1.XTick = [t0:dt_dyn:T]; 101 | 102 | 103 | sp2 = subplot(3,1,2); 104 | [he0t, he0z, he0f] = plotyy(t, foot_0_z, t, force_0_z); 105 | set(he0t(1),'XLim',[t0 T]); 106 | set(he0t(2),'XLim',[t0 T]); 107 | set(he0t(2),'YLim',[-80 600]); 108 | set(he0t(2),'YTick',[-200:200:600]); 109 | sp2.XGrid = 'on'; 110 | sp2.XTick = dt_foot_0; 111 | 112 | 113 | sp3 = subplot(3,1,3); 114 | [he1t, he1z, he1f] = plotyy(t, foot_1_z, t, force_1_z); 115 | set(he1t(1),'XLim',[t0 T]); 116 | set(he1t(2),'XLim',[t0 T]); 117 | set(he1t(2),'YLim',[-80 600]); 118 | set(he1t(2),'YTick',[-200:200:600]); 119 | sp3.XGrid = 'on'; 120 | sp3.XTick = dt_foot_1; 121 | 122 | 123 | 124 | %% Generate pdf from the figure for paper 125 | width = 20; 126 | height = 15; 127 | 128 | fh.Units = 'centimeters'; 129 | fh.PaperUnits = 'centimeters'; 130 | fh.Position = [0, 0, width, height]; 131 | fh.PaperSize = [width, height]; 132 | fh.PaperPositionMode = 'auto'; 133 | fn = 'side_stepping'; 134 | 135 | saveas(fh, fn, 'pdf') 136 | system(['pdfcrop ' fn ' ' fn]); 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /towr/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package towr 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2019-04-05) 6 | ------------------ 7 | * Merge pull request (`#56 `_) from ethz-adrl/expose-params 8 | * add explanation for assert in phase_durations 9 | * Merge branch 'sweetie-bot-project-feat/access-optimization-parameteres' into expose-params 10 | * Optimize GetPhaseDurations(). Remove unnecessary GetNormalizedPhaseDurations(). 11 | * Make GaitGenerator::SetGaits() method public. 12 | * Move default parameter values to header. 13 | * Use pair instead of array to store bounds. 14 | * Expose contstraints and costs field to user. Remove unnecessary private functions. 15 | * Add missing underscore postfix for bounds_final variables. 16 | * Take into account weights of the costs. (`#51 `_) 17 | * Contributors: Alexander Winkler, Mathieu Geisert, awinkler, disRecord 18 | 19 | 1.4.0 (2018-07-30) 20 | ------------------ 21 | * Simplify extension / adding own formulation (`#38 `_) 22 | * Facilitate towr_ros user extension (`#34 `_) 23 | * Greatly simplify phase node formulations (`#33 `_) 24 | * keep overview in github readme, not separated into doxygen & github 25 | * rename CD to SRBD and improve documentation 26 | * rename nodes to node variables 27 | * Contributors: Alexander Winkler 28 | 29 | 1.3.2 (2018-07-17) 30 | ------------------ 31 | * adapt to more generic ifopt solver interface. 32 | * Improve doxygen (`#26 `_) 33 | * add overview on main doxygen landing 34 | * add doxygen groups to variables/constraints/costs 35 | * add parameter explanation 36 | * Contributors: Alexander Winkler 37 | 38 | 1.3.1 (2018-07-10) 39 | ------------------ 40 | * Improve API (`#23 `_) 41 | * Remove redundant total time (duration set by endeffectors) 42 | * Contributors: Alexander Winkler 43 | 44 | 1.3.0 (2018-07-07) 45 | ------------------ 46 | * add sample gaits for mono-, bi- and quadruped 47 | * Contributors: Alexander Winkler 48 | 49 | 1.2.2 (2018-07-03) 50 | ------------------ 51 | * remove controller specifc code from towr_ros 52 | * moved height map from towr_ros to towr 53 | * moved robots models and gait generator from towr_ros to towr 54 | * move dynamic and kinematic models from towr_ros -> towr 55 | * remove all catkin macros from towr::CMakeLists.txt 56 | * Contributors: Alexander Winkler 57 | 58 | 1.2.1 (2018-06-30) 59 | ------------------ 60 | * set parameters for hyq and terrains examples 61 | * rename constraints and variables for more consistency 62 | * renamed main library (towr_core -> towr) and removed ros meta package 63 | * Contributors: Alexander Winkler 64 | 65 | 1.2.0 (2018-06-25) 66 | ------------------ 67 | * allow building with pure cmake (catkin optional) 68 | * adapt to version 2.0.0 of ifopt (`#17 `_) 69 | * add derivative of system dynamics w.r.t angular orientation 70 | * Improve centroidal dynamics model and add continuous base acceleration constraint 71 | * Fix final base and footholds through constraint 72 | * Separate ifopt solver from towr and towr to header+source file 73 | * Contributors: Alexander Winkler 74 | 75 | 1.1.0 (2018-02-06) 76 | ------------------ 77 | * add metapackage towr and move algorithm to towr_core 78 | * create separate ros independent example package "towr_ros" 79 | * replaced ros-keyboard dependency with ncurses 80 | * moved all robot specific model/gait generators out of towr -> towr_ros 81 | * use only one unified represenatation for nodes and states 82 | * remove xpp states dependency 83 | * added base_nodes class that derives from node_variables 84 | * add observer pattern (spline observes node_values and contact_schedule) 85 | * added spline_holder to not always have to reconstruct from variables 86 | * separated spline and node values 87 | * adapted to changed ifopt namespace (opt -> ifopt) 88 | * removed unused variables in polynomial 89 | * renamed pkg from xpp_opt to towr 90 | * Contributors: Alexander Winkler 91 | 92 | 1.0.0 (2017-09-19) 93 | ------------------ 94 | -------------------------------------------------------------------------------- /towr/src/phase_spline.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | namespace towr { 34 | 35 | PhaseSpline::PhaseSpline( 36 | NodesVariablesPhaseBased::Ptr const nodes, 37 | PhaseDurations* const phase_durations) 38 | : NodeSpline(nodes.get(), nodes->ConvertPhaseToPolyDurations(phase_durations->GetPhaseDurations())), 39 | PhaseDurationsObserver(phase_durations) 40 | { 41 | phase_nodes_ = nodes; 42 | 43 | UpdatePolynomialDurations(); 44 | 45 | // if durations change, the polynomial active at a specified global time 46 | // changes. Therefore, all elements of the Jacobian could be non-zero 47 | // and must make sure that Jacobian structure never changes during 48 | // the iterations. 49 | // assume every global time time can fall into every polynomial 50 | for (int i=0; iGetPolynomialCount(); ++i) 51 | FillJacobianWrtNodes(i, 0.0, kPos, jac_wrt_nodes_structure_, true); 52 | } 53 | 54 | void 55 | PhaseSpline::UpdatePolynomialDurations() 56 | { 57 | auto phase_duration = phase_durations_->GetPhaseDurations(); 58 | auto poly_durations = phase_nodes_->ConvertPhaseToPolyDurations(phase_duration); 59 | 60 | for (int i=0; iGetPhaseDurations()); 73 | 74 | return phase_durations_->GetJacobianOfPos(current_phase, dx_dT, xd); 75 | } 76 | 77 | Eigen::VectorXd 78 | PhaseSpline::GetDerivativeOfPosWrtPhaseDuration (double t_global) const 79 | { 80 | int poly_id; double t_local; 81 | std::tie(poly_id, t_local) = GetLocalTime(t_global, GetPolyDurations()); 82 | 83 | VectorXd vel = GetPoint(t_global).v(); 84 | VectorXd dxdT = cubic_polys_.at(poly_id).GetDerivativeOfPosWrtDuration(t_local); 85 | 86 | double inner_derivative = phase_nodes_->GetDerivativeOfPolyDurationWrtPhaseDuration(poly_id); 87 | double prev_polys_in_phase = phase_nodes_->GetNumberOfPrevPolynomialsInPhase(poly_id); 88 | 89 | // where this minus term comes from: 90 | // from number of polynomials before current polynomial that 91 | // cause shifting of entire spline 92 | return inner_derivative*(dxdT - prev_polys_in_phase*vel); 93 | } 94 | 95 | 96 | } /* namespace towr */ 97 | -------------------------------------------------------------------------------- /towr/include/towr/variables/spline.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_VARIABLES_SPLINE_H_ 31 | #define TOWR_VARIABLES_SPLINE_H_ 32 | 33 | #include 34 | 35 | #include "polynomial.h" 36 | 37 | namespace towr { 38 | 39 | /** 40 | * @brief A spline built from a sequence of cubic polynomials. 41 | * 42 | * This class is responsible for stitching together multiple individual 43 | * polynomials into one spline. 44 | */ 45 | class Spline { 46 | public: 47 | using VecTimes = std::vector; 48 | using VecPoly = std::vector; 49 | 50 | Spline(const VecTimes& poly_durations, int n_dim); 51 | virtual ~Spline () = default; 52 | 53 | /** 54 | * @returns The state of the spline at time t. 55 | * @param t The time at which the state of the spline is desired. 56 | */ 57 | const State GetPoint(double t) const; 58 | 59 | /** 60 | * @param poly_id Polynomial id, 0 is first polynomial. 61 | * @param t_local Time along the current polynomial. 62 | * @returns The position, velocity and acceleration of spline. 63 | */ 64 | const State GetPoint(int poly_id, double t_local) const; 65 | 66 | /** 67 | * @returns The segment (e.g. phase, polynomial) at time t_global. 68 | * @param t_global The global time in the spline. 69 | * @param durations The durations [s] of each segment. 70 | */ 71 | static int GetSegmentID(double t_global, const VecTimes& durations); 72 | 73 | /** 74 | * @returns The total time of the spline. 75 | */ 76 | double GetTotalTime() const; 77 | 78 | /** 79 | * @returns the number of polynomials making up this spline. 80 | */ 81 | int GetPolynomialCount() const; 82 | 83 | /** 84 | * @returns the durations of each polynomial. 85 | */ 86 | VecTimes GetPolyDurations() const; 87 | 88 | protected: 89 | VecPoly cubic_polys_; ///< the sequence of polynomials making up the spline. 90 | 91 | /** 92 | * @brief How much time of the current segment has passed at t_global. 93 | * @param t_global The global time [s] along the spline. 94 | * @param d The durations of each segment. 95 | * @return The segment id and the time passed in this segment. 96 | */ 97 | std::pair GetLocalTime(double t_global, const VecTimes& d) const; 98 | 99 | /** 100 | * @brief Updates the cubic-Hermite polynomial coefficients using the 101 | * currently set nodes values and durations. 102 | */ 103 | void UpdatePolynomialCoeff(); 104 | }; 105 | 106 | } /* namespace towr */ 107 | 108 | #endif /* TOWR_VARIABLES_SPLINE_H_ */ 109 | -------------------------------------------------------------------------------- /towr_ros/src/rviz_terrain_publisher.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | namespace towr { 41 | 42 | static ros::Publisher rviz_pub; 43 | 44 | void UserCommandCallback(const towr_ros::TowrCommand& msg_in) 45 | { 46 | // get which terrain 47 | auto terrain_id = static_cast(msg_in.terrain); 48 | auto terrain_ = HeightMap::MakeTerrain(terrain_id); 49 | 50 | // x-y area patch that should be drawn in rviz 51 | double dxy = 0.06; 52 | double x_min = -1.0; 53 | double x_max = 4.0; 54 | double y_min = -1.0; 55 | double y_max = 1.0; 56 | 57 | visualization_msgs::Marker m; 58 | int id = 0; 59 | m.type = visualization_msgs::Marker::CUBE; 60 | m.scale.z = 0.003; 61 | m.ns = "terrain"; 62 | m.header.frame_id = "world"; 63 | m.color.r = 245./355; m.color.g = 222./355; m.color.b = 179./355; // wheat 64 | m.color.a = 0.65; 65 | 66 | visualization_msgs::MarkerArray msg; 67 | double x = x_min; 68 | while (x < x_max) { 69 | double y = y_min; 70 | while (y < y_max) { 71 | // position 72 | m.pose.position.x = x; 73 | m.pose.position.y = y; 74 | m.pose.position.z = terrain_->GetHeight(x,y); 75 | 76 | // orientation 77 | Eigen::Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, x, y); 78 | Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0,0,1), n); 79 | m.pose.orientation.w = q.w(); 80 | m.pose.orientation.x = q.x(); 81 | m.pose.orientation.y = q.y(); 82 | m.pose.orientation.z = q.z(); 83 | 84 | // enlarge surface-path when tilting 85 | double gain = 1.5; 86 | m.scale.x = (1+gain*n.cwiseAbs().x())*dxy; 87 | m.scale.y = (1+gain*n.cwiseAbs().y())*dxy; 88 | 89 | 90 | m.id = id++; 91 | msg.markers.push_back(m); 92 | 93 | y += dxy; 94 | } 95 | x += dxy; 96 | } 97 | 98 | rviz_pub.publish(msg); 99 | } 100 | 101 | } // namespace towr 102 | 103 | int main(int argc, char *argv[]) 104 | { 105 | ros::init(argc, argv, "rviz_terrain_visualizer"); 106 | 107 | ros::NodeHandle n; 108 | 109 | ros::Subscriber goal_sub; 110 | goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); 111 | towr::rviz_pub = n.advertise("xpp/terrain", 1); 112 | 113 | ros::spin(); 114 | 115 | return 1; 116 | } 117 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/range_of_motion_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "time_discretization_constraint.h" 40 | 41 | namespace towr { 42 | 43 | /** @brief Constrains an endeffector to lie in a box around the nominal stance. 44 | * 45 | * These constraints are necessary to avoid configurations 46 | * that are outside the kinematic reach of the robot. The constraint 47 | * is defined by Cartesian estimates of the reachability of each endeffector. 48 | * 49 | * This constraint calculates the position of of the contact expressed in the 50 | * current CoM frame and constrains it to lie in a box around the nominal/ 51 | * natural contact position for that leg. 52 | * 53 | * @ingroup Constraints 54 | */ 55 | class RangeOfMotionConstraint : public TimeDiscretizationConstraint { 56 | public: 57 | using EE = uint; 58 | using Vector3d = Eigen::Vector3d; 59 | 60 | /** 61 | * @brief Constructs a constraint instance. 62 | * @param robot_model The kinematic restrictions of the robot. 63 | * @param T The total duration of the optimization. 64 | * @param dt the discretization intervall at which to enforce constraints. 65 | * @param ee The endeffector for which to constrain the range. 66 | * @param spline_holder Pointer to the current variables. 67 | */ 68 | RangeOfMotionConstraint(const KinematicModel::Ptr& robot_model, 69 | double T, double dt, 70 | const EE& ee, 71 | const SplineHolder& spline_holder); 72 | virtual ~RangeOfMotionConstraint() = default; 73 | 74 | private: 75 | NodeSpline::Ptr base_linear_; ///< the linear position of the base. 76 | EulerConverter base_angular_; ///< the orientation of the base. 77 | NodeSpline::Ptr ee_motion_; ///< the linear position of the endeffectors. 78 | 79 | Eigen::Vector3d max_deviation_from_nominal_; 80 | Eigen::Vector3d nominal_ee_pos_B_; 81 | EE ee_; 82 | 83 | // see TimeDiscretizationConstraint for documentation 84 | void UpdateConstraintAtInstance (double t, int k, VectorXd& g) const override; 85 | void UpdateBoundsAtInstance (double t, int k, VecBound&) const override; 86 | void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; 87 | 88 | int GetRow(int node, int dimension) const; 89 | }; 90 | 91 | } /* namespace towr */ 92 | 93 | #endif /* TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ */ 94 | -------------------------------------------------------------------------------- /towr/include/towr/constraints/force_constraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef TOWR_CONSTRAINTS_FORCE_CONSTRAINT_H_ 31 | #define TOWR_CONSTRAINTS_FORCE_CONSTRAINT_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include // for friction cone 37 | 38 | namespace towr { 39 | 40 | /** 41 | * @brief Ensures foot force that is unilateral and inside friction cone. 42 | * 43 | * This class is responsible for constraining the endeffector xyz-forces to 44 | * only push into the terrain and additionally stay inside the friction cone 45 | * according to the current slope. 46 | * 47 | * In order to keep the constraint linear and simple for the solver to solve, 48 | * we approximate the friction cone by a 4-sided pyramid. 49 | * 50 | * Attention: Constraint is enforced only at the spline nodes. In between 51 | * violations of this constraint can occur. 52 | * 53 | * @ingroup Constraints 54 | */ 55 | class ForceConstraint : public ifopt::ConstraintSet { 56 | public: 57 | using Vector3d = Eigen::Vector3d; 58 | using EE = uint; 59 | 60 | /** 61 | * @brief Constructs a force contraint. 62 | * @param terrain The gradient information of the terrain for friction cone. 63 | * @param force_limit_in_normal_direction Maximum pushing force [N]. 64 | * @param endeffector_id Which endeffector force should be constrained. 65 | */ 66 | ForceConstraint (const HeightMap::Ptr& terrain, 67 | double force_limit_in_normal_direction, 68 | EE endeffector_id); 69 | virtual ~ForceConstraint () = default; 70 | 71 | void InitVariableDependedQuantities(const VariablesPtr& x) override; 72 | 73 | VectorXd GetValues() const override; 74 | VecBound GetBounds() const override; 75 | void FillJacobianBlock (std::string var_set, Jacobian&) const override; 76 | 77 | private: 78 | NodesVariablesPhaseBased::Ptr ee_force_; ///< the current xyz foot forces. 79 | NodesVariablesPhaseBased::Ptr ee_motion_; ///< the current xyz foot positions. 80 | 81 | HeightMap::Ptr terrain_; ///< gradient information at every position (x,y). 82 | double fn_max_; ///< force limit in normal direction. 83 | double mu_; ///< friction coeff between robot feet and terrain. 84 | int n_constraints_per_node_; ///< number of constraint for each node. 85 | EE ee_; ///< The endeffector force to be constrained. 86 | 87 | /** 88 | * The are those Hermite-nodes that shape the polynomial during the 89 | * stance phases, while all the others are already set to zero force (swing) 90 | **/ 91 | std::vector pure_stance_force_node_ids_; 92 | }; 93 | 94 | } /* namespace towr */ 95 | 96 | #endif /* TOWR_CONSTRAINTS_FORCE_CONSTRAINT_H_ */ 97 | -------------------------------------------------------------------------------- /towr/src/node_spline.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace towr { 35 | 36 | NodeSpline::NodeSpline(NodeSubjectPtr const node_variables, 37 | const VecTimes& polynomial_durations) 38 | : Spline(polynomial_durations, node_variables->GetDim()), 39 | NodesObserver(node_variables) 40 | { 41 | UpdateNodes(); 42 | jac_wrt_nodes_structure_ = Jacobian(node_variables->GetDim(), node_variables->GetRows()); 43 | } 44 | 45 | void 46 | NodeSpline::UpdateNodes () 47 | { 48 | for (int i=0; iGetBoundaryNodes(i); 50 | cubic_polys_.at(i).SetNodes(nodes.front(), nodes.back()); 51 | } 52 | 53 | UpdatePolynomialCoeff(); 54 | } 55 | 56 | int 57 | NodeSpline::GetNodeVariablesCount() const 58 | { 59 | return node_values_->GetRows(); 60 | } 61 | 62 | NodeSpline::Jacobian 63 | NodeSpline::GetJacobianWrtNodes (double t_global, Dx dxdt) const 64 | { 65 | int id; double t_local; 66 | std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations()); 67 | 68 | return GetJacobianWrtNodes(id, t_local, dxdt); 69 | } 70 | 71 | NodeSpline::Jacobian 72 | NodeSpline::GetJacobianWrtNodes (int id, double t_local, Dx dxdt) const 73 | { 74 | Jacobian jac = jac_wrt_nodes_structure_; 75 | FillJacobianWrtNodes(id, t_local, dxdt, jac, false); 76 | 77 | // needed to avoid Eigen::assert failure "wrong storage order" triggered 78 | // in dynamic_constraint.cc 79 | jac.makeCompressed(); 80 | 81 | return jac; 82 | } 83 | 84 | void 85 | NodeSpline::FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt, 86 | Jacobian& jac, bool fill_with_zeros) const 87 | { 88 | for (int idx=0; idxGetNodeValuesInfo(idx)) { 90 | for (auto side : {NodesVariables::Side::Start, NodesVariables::Side::End}) { // every jacobian is affected by two nodes 91 | int node = node_values_->GetNodeId(poly_id, side); 92 | 93 | if (node == nvi.id_) { 94 | double val = 0.0; 95 | 96 | if (side == NodesVariables::Side::Start) 97 | val = cubic_polys_.at(poly_id).GetDerivativeWrtStartNode(dxdt, nvi.deriv_, t_local); 98 | else if (side == NodesVariables::Side::End) 99 | val = cubic_polys_.at(poly_id).GetDerivativeWrtEndNode(dxdt, nvi.deriv_, t_local); 100 | else 101 | assert(false); // this shouldn't happen 102 | 103 | // if only want structure 104 | if (fill_with_zeros) 105 | val = 0.0; 106 | 107 | jac.coeffRef(nvi.dim_, idx) += val; 108 | } 109 | } 110 | } 111 | } 112 | } 113 | 114 | } /* namespace towr */ 115 | --------------------------------------------------------------------------------