diff --git a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h index 6adb50176..7c2747c63 100644 --- a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h +++ b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h @@ -40,7 +40,7 @@ SCALAR_T trapezoidalIntegration(const std::vector& timeTrajectory, con } SCALAR_T areaUnderCurve = 0.0; - for (size_t k = 1; k < timeTrajectory.size(); k++) { + for (std::size_t k = 1; k < timeTrajectory.size(); k++) { areaUnderCurve += 0.5 * (valueTrajectory[k] + valueTrajectory[k - 1]) * (timeTrajectory[k] - timeTrajectory[k - 1]); } // end of k loop diff --git a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt index 73b97f736..78f5c7b0a 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt @@ -15,7 +15,8 @@ find_package(ocs2_core REQUIRED) find_package(ocs2_robotic_tools REQUIRED) find_package(ocs2_pinocchio_interface REQUIRED) find_package(OpenMP REQUIRED) - +find_package(urdf) +find_package(urdfdom) find_package(Boost REQUIRED COMPONENTS system filesystem @@ -66,6 +67,8 @@ ament_target_dependencies(${PROJECT_NAME} ocs2_pinocchio_interface ocs2_core ocs2_robotic_tools + urdf + urdfdom ) set_target_properties(${PROJECT_NAME} @@ -111,6 +114,8 @@ ament_export_dependencies(ocs2_core) ament_export_dependencies(ocs2_robotic_tools) ament_export_dependencies(ocs2_thirdparty) ament_export_dependencies(ocs2_pinocchio_interface) +ament_export_dependencies(urdf) +ament_export_dependencies(urdfdom) ############# ## Testing ## diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt index be0de478a..6e8a82097 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(ocs2_robotic_tools REQUIRED) find_package(ocs2_oc REQUIRED) find_package(ocs2_thirdparty REQUIRED) find_package(OpenMP REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) find_package(Boost REQUIRED COMPONENTS system @@ -77,6 +79,8 @@ ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_oc ocs2_robotic_tools + urdf + urdfdom ) target_link_libraries(${PROJECT_NAME} @@ -127,6 +131,8 @@ ament_export_targets( ament_export_dependencies(ocs2_core) ament_export_dependencies(ocs2_robotic_tools) +ament_export_dependencies(urdf) +ament_export_dependencies(urdfdom) ############ # Testing ## diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml index 7bb55540d..35ba11943 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml @@ -15,6 +15,8 @@ ocs2_core ocs2_oc ocs2_robotic_tools + urdf + urdfdom pinocchio diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index e6e3f8dd2..23abe471b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -46,6 +46,7 @@ set(AMENT_PACKAGE_DEPENDENCIES tf2 tf2_ros urdf + urdfdom kdl_parser rclcpp robot_state_publisher @@ -307,6 +308,7 @@ ament_export_dependencies(ocs2_centroidal_model) ament_export_dependencies(ocs2_ros_interfaces) ament_export_dependencies(tf2) ament_export_dependencies(urdf) +ament_export_dependencies(urdfdom) ament_export_dependencies(kdl_parser) ament_export_dependencies(robot_state_publisher) ament_export_dependencies(visualization_msgs) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp index 67447c31c..0090f9b19 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp @@ -163,8 +163,7 @@ void LeggedRobotVisualizer::publishTrajectory(const std::vectornow(), system_observation_array[k]); }); if (frameDuration > publishDuration) { - rclcpp::Duration dur(frameDuration - publishDuration); - rclcpp::sleep_for(std::chrono::nanoseconds(dur.nanoseconds())); + rclcpp::sleep_for(std::chrono::nanoseconds(int((frameDuration - publishDuration)*1e9))); } } } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index 9ddb1cc68..18a694283 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -52,7 +52,7 @@ MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesir /******************************************************************************************************/ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); - const scalar_t timeStep = (1.0 / mrtDesiredFrequency_); + const double timeStep = (1.0 / mrtDesiredFrequency_); // Reset MPC node mrt_.resetMpcNode(initTargetTrajectories); @@ -61,8 +61,7 @@ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const Tar mrt_.spinMRT(); mrt_.setCurrentObservation(initObservation); - int16_t sleeptime = rclcpp::Duration(timeStep).nanoseconds(); - rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime)); + rclcpp::sleep_for(std::chrono::nanoseconds(int(timeStep * 1e9))); } RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received.");