commit fd8b4d38a248f94356512c46de2996faccc69594 Author: dzp <1847380236@qq.com> Date: Wed Nov 6 09:43:33 2024 +0800 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6831c06 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,162 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_mobile_manipulator_ros) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(CATKIN_PACKAGE_DEPENDENCIES + roslib + tf + urdf + kdl_parser + robot_state_publisher + visualization_msgs + geometry_msgs + ocs2_ros_interfaces + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + ocs2_self_collision + ocs2_self_collision_visualization + ocs2_mobile_manipulator +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(pinocchio REQUIRED pinocchio) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + Boost + pinocchio +) + +########### +## Build ## +########### + +set(FLAGS + ${OCS2_CXX_FLAGS} + ${pinocchio_CFLAGS_OTHER} + -Wno-ignored-attributes + -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor + -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR + -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) + +link_directories( + ${pinocchio_LIBRARY_DIRS} +) + +# MPC node +add_executable(mobile_manipulator_mpc_node + src/MobileManipulatorMpcNode.cpp +) +add_dependencies(mobile_manipulator_mpc_node + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(mobile_manipulator_mpc_node + ${catkin_LIBRARIES} +) +target_compile_options(mobile_manipulator_mpc_node PUBLIC ${FLAGS}) + +# DistanceVisualization node +add_executable(mobile_manipulator_distance_visualization + src/MobileManipulatorDistanceVisualization.cpp +) +add_dependencies(mobile_manipulator_distance_visualization + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(mobile_manipulator_distance_visualization + ${catkin_LIBRARIES} +) +target_compile_options(mobile_manipulator_distance_visualization PUBLIC ${FLAGS}) + +# Dummy node +add_executable(mobile_manipulator_dummy_mrt_node + src/MobileManipulatorDummyMRT.cpp + src/MobileManipulatorDummyVisualization.cpp +) +add_dependencies(mobile_manipulator_dummy_mrt_node + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(mobile_manipulator_dummy_mrt_node + ${catkin_LIBRARIES} +) +target_compile_options(mobile_manipulator_dummy_mrt_node PUBLIC ${FLAGS}) + +# Target node +add_executable(mobile_manipulator_target + src/MobileManipulatorTarget.cpp +) +add_dependencies(mobile_manipulator_target + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(mobile_manipulator_target + ${catkin_LIBRARIES} +) +target_compile_options(mobile_manipulator_target PUBLIC ${FLAGS}) + +#################### +## Clang tooling ### +#################### + +find_package(cmake_clang_tools QUIET) +if (cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling") + add_clang_tooling( + TARGETS mobile_manipulator_mpc_node mobile_manipulator_dummy_mrt_node + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif (cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) +install( + TARGETS + mobile_manipulator_mpc_node + mobile_manipulator_distance_visualization + mobile_manipulator_dummy_mrt_node + mobile_manipulator_target + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h b/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h new file mode 100644 index 0000000..aeb71df --- /dev/null +++ b/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h @@ -0,0 +1,76 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include +#include +#include + +namespace ocs2 { +namespace mobile_manipulator { + +class MobileManipulatorDummyVisualization final : public DummyObserver { + public: + MobileManipulatorDummyVisualization(ros::NodeHandle& nodeHandle, const MobileManipulatorInterface& interface) + : pinocchioInterface_(interface.getPinocchioInterface()), modelInfo_(interface.getManipulatorModelInfo()) { + launchVisualizerNode(nodeHandle); + } + + ~MobileManipulatorDummyVisualization() override = default; + + void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + + private: + void launchVisualizerNode(ros::NodeHandle& nodeHandle); + + void publishObservation(const ros::Time& timeStamp, const SystemObservation& observation); + void publishTargetTrajectories(const ros::Time& timeStamp, const TargetTrajectories& targetTrajectories); + void publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy); + + PinocchioInterface pinocchioInterface_; + const ManipulatorModelInfo modelInfo_; + std::vector removeJointNames_; + + std::unique_ptr robotStatePublisherPtr_; + tf::TransformBroadcaster tfBroadcaster_; + + ros::Publisher stateOptimizedPublisher_; + ros::Publisher stateOptimizedPosePublisher_; + + std::unique_ptr geometryVisualization_; +}; + +} // namespace mobile_manipulator +} // namespace ocs2 diff --git a/launch/aloha.launch b/launch/aloha.launch new file mode 100644 index 0000000..79574ba --- /dev/null +++ b/launch/aloha.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/include/mobile_manipulator.launch b/launch/include/mobile_manipulator.launch new file mode 100644 index 0000000..9f02a6f --- /dev/null +++ b/launch/include/mobile_manipulator.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/include/mobile_manipulator_distance.launch b/launch/include/mobile_manipulator_distance.launch new file mode 100644 index 0000000..458cfa0 --- /dev/null +++ b/launch/include/mobile_manipulator_distance.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/include/visualize.launch b/launch/include/visualize.launch new file mode 100644 index 0000000..573fb8c --- /dev/null +++ b/launch/include/visualize.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/manipulator_franka.launch b/launch/manipulator_franka.launch new file mode 100644 index 0000000..3ea4e52 --- /dev/null +++ b/launch/manipulator_franka.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/manipulator_kinova_j2n6.launch b/launch/manipulator_kinova_j2n6.launch new file mode 100644 index 0000000..97d2908 --- /dev/null +++ b/launch/manipulator_kinova_j2n6.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/manipulator_kinova_j2n7.launch b/launch/manipulator_kinova_j2n7.launch new file mode 100644 index 0000000..c5a4e47 --- /dev/null +++ b/launch/manipulator_kinova_j2n7.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/manipulator_mabi_mobile.launch b/launch/manipulator_mabi_mobile.launch new file mode 100644 index 0000000..21041af --- /dev/null +++ b/launch/manipulator_mabi_mobile.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/manipulator_pr2.launch b/launch/manipulator_pr2.launch new file mode 100644 index 0000000..c69225c --- /dev/null +++ b/launch/manipulator_pr2.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/manipulator_realman.launch b/launch/manipulator_realman.launch new file mode 100644 index 0000000..c90c7df --- /dev/null +++ b/launch/manipulator_realman.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/manipulator_ridgeback_ur5.launch b/launch/manipulator_ridgeback_ur5.launch new file mode 100644 index 0000000..369947f --- /dev/null +++ b/launch/manipulator_ridgeback_ur5.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..3a68339 --- /dev/null +++ b/package.xml @@ -0,0 +1,36 @@ + + + ocs2_mobile_manipulator_ros + 0.0.0 + The ocs2_mobile_manipulator_ros package + + Farbod Farshidian + Michael Spieler + Perry Franklin + Mayank Mittal + + BSD3 + + catkin + cmake_clang_tools + + roslib + tf + urdf + kdl_parser + robot_state_publisher + visualization_msgs + geometry_msgs + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_ros_interfaces + ocs2_pinocchio_interface + ocs2_self_collision + ocs2_self_collision_visualization + ocs2_mobile_manipulator + pinocchio + + diff --git a/resources/aloha/meshes/wx250_10_ar_tag.stl b/resources/aloha/meshes/wx250_10_ar_tag.stl new file mode 100644 index 0000000..193014b Binary files /dev/null and b/resources/aloha/meshes/wx250_10_ar_tag.stl differ diff --git a/resources/aloha/meshes/wx250_1_base.stl b/resources/aloha/meshes/wx250_1_base.stl new file mode 100644 index 0000000..ff1b0bc Binary files /dev/null and b/resources/aloha/meshes/wx250_1_base.stl differ diff --git a/resources/aloha/meshes/wx250_2_shoulder.stl b/resources/aloha/meshes/wx250_2_shoulder.stl new file mode 100644 index 0000000..83df495 Binary files /dev/null and b/resources/aloha/meshes/wx250_2_shoulder.stl differ diff --git a/resources/aloha/meshes/wx250_3_upper_arm.stl b/resources/aloha/meshes/wx250_3_upper_arm.stl new file mode 100644 index 0000000..4d7c8fc Binary files /dev/null and b/resources/aloha/meshes/wx250_3_upper_arm.stl differ diff --git a/resources/aloha/meshes/wx250_4_forearm.stl b/resources/aloha/meshes/wx250_4_forearm.stl new file mode 100644 index 0000000..9381e0f Binary files /dev/null and b/resources/aloha/meshes/wx250_4_forearm.stl differ diff --git a/resources/aloha/meshes/wx250_5_wrist.stl b/resources/aloha/meshes/wx250_5_wrist.stl new file mode 100644 index 0000000..32fdf2b Binary files /dev/null and b/resources/aloha/meshes/wx250_5_wrist.stl differ diff --git a/resources/aloha/meshes/wx250_6_gripper.stl b/resources/aloha/meshes/wx250_6_gripper.stl new file mode 100644 index 0000000..ee21687 Binary files /dev/null and b/resources/aloha/meshes/wx250_6_gripper.stl differ diff --git a/resources/aloha/meshes/wx250_7_gripper_prop.stl b/resources/aloha/meshes/wx250_7_gripper_prop.stl new file mode 100644 index 0000000..2124443 Binary files /dev/null and b/resources/aloha/meshes/wx250_7_gripper_prop.stl differ diff --git a/resources/aloha/meshes/wx250_8_gripper_bar.stl b/resources/aloha/meshes/wx250_8_gripper_bar.stl new file mode 100644 index 0000000..5b91cbf Binary files /dev/null and b/resources/aloha/meshes/wx250_8_gripper_bar.stl differ diff --git a/resources/aloha/meshes/wx250_9_gripper_finger.stl b/resources/aloha/meshes/wx250_9_gripper_finger.stl new file mode 100644 index 0000000..a568b08 Binary files /dev/null and b/resources/aloha/meshes/wx250_9_gripper_finger.stl differ diff --git a/resources/aloha/urdf/wx250.urdf b/resources/aloha/urdf/wx250.urdf new file mode 100644 index 0000000..f52c500 --- /dev/null +++ b/resources/aloha/urdf/wx250.urdf @@ -0,0 +1,399 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/config/aloha/aloha.info b/resources/config/aloha/aloha.info new file mode 100644 index 0000000..1d0194c --- /dev/null +++ b/resources/config/aloha/aloha.info @@ -0,0 +1,321 @@ +; robot model meta-information +model_information { + manipulatorModelType 0 // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator + + ; motion joints in the URDF to consider fixed + removeJoints { + [0] "gripper" + [1] "gripper_bar" + [2] "ee_bar" + [3] "left_finger" + [4] "right_finger" + [5] "ee_gripper" + [6] "root_joint" + + } + + ; base frame of the robot (from URDF) + baseFrame "root" + ; end-effector frame of the robot (from URDF) + eeFrame "wx250/gripper_prop_link" +} + +model_settings +{ + usePreComputation true + recompileLibraries true +} + +; DDP settings +ddp +{ + algorithm SLQ + + nThreads 3 + threadPriority 50 + + maxNumIterations 1 + minRelCost 0.1 + constraintTolerance 1e-3 + + displayInfo false + displayShortSummary false + checkNumericalStability false + debugPrintRollout false + debugCaching false + + AbsTolODE 1e-5 + RelTolODE 1e-3 + maxNumStepsPerSecond 100000 + timeStep 1e-3 + backwardPassIntegratorType ODE45 + + constraintPenaltyInitialValue 20.0 + constraintPenaltyIncreaseRate 2.0 + + preComputeRiccatiTerms true + + useFeedbackPolicy false + + strategy LINE_SEARCH + lineSearch + { + minStepLength 1e-2 + maxStepLength 1.0 + hessianCorrectionStrategy DIAGONAL_SHIFT + hessianCorrectionMultiple 1e-3 + } +} + +; Rollout settings +rollout +{ + AbsTolODE 1e-5 + RelTolODE 1e-3 + timeStep 1e-2 + integratorType ODE45 + maxNumStepsPerSecond 100000 + checkNumericalStability false +} + +; MPC settings +mpc +{ + timeHorizon 1.0 ; [s] + solutionTimeWindow 0.2 ; [s] + coldStart false + + debugPrint false + + mpcDesiredFrequency 100 ; [Hz] + mrtDesiredFrequency 400 ; [Hz] +} + +; initial state +initialState +{ + ; initial state for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; heading + } + } + + ; initial state for the arm DOFs + arm + { + (0,0) -0.56 ; arm_1 + (1,0) 1.65 ; arm_2 + (2,0) -1.71 ; arm_3 + (3,0) -1.07 ; arm_4 + (4,0) 0.09 ; arm_5 + (5,0) 0.19 ; arm_6 + } +} + +inputCost +{ + ; control weight matrixarmarm + + R + { + ; input costs for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + scaling 1e-2 + + (0,0) 5.0 ; position x + (1,1) 5.0 ; position y + (2,2) 5.0 ; position z + (3,3) 5.0 ; euler angle z + (4,4) 5.0 ; euler angle y + (5,5) 5.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + scaling 1e-2 + + (0,0) 5.0 ; forward velocity + (1,1) 5.0 ; turning velocity + } + } + + ; input costs for the arm DOFs + arm + { + scaling 1e-2 + + (0,0) 1.0 ; arm_1 velocity + (1,1) 1.0 ; arm_2 velocity + (2,2) 1.0 ; arm_3 velocity + (3,3) 1.0 ; arm_4 velocity + (4,4) 1.0 ; arm_5 velocity + (5,5) 1.0 ; arm_6 velocity + } + } +} +endEffector +{ + ; end effector quadratic penalty scaling + muPosition 10.0 + muOrientation 5.0 +} + +finalEndEffector +{ + muPosition 10.0 + muOrientation 5.0 +} + +selfCollision +{ + ; activate self-collision constraint + activate false + + ; TODO: Replace the collision meshes of the arm with primitive shapes. +} + +; Only applied for arm joints: limits parsed from URDF +jointPositionLimits +{ + ; activate constraint + activate true + + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 +} + +jointVelocityLimits +{ + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 + + lowerBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) -0.1 ; linear velocity x + (1,0) -0.1 ; linear velocity y + (2,0) -0.1 ; linear velocity z + (3,0) -0.3 ; euler angle velocity z + (4,0) -0.3 ; euler angle velocity y + (5,0) -0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) -0.1 ; forward velocity + (1,0) -0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs + arm + { + (0,0) -15.0 ; arm_1 [rad/s] + (1,0) -15.0 ; arm_2 [rad/s] + (2,0) -15.0 ; arm_3 [rad/s] + (3,0) -15.0 ; arm_4 [rad/s] + (4,0) -15.0 ; arm_5 [rad/s] + (5,0) -15.0 ; arm_6 [rad/s] + } + } + + upperBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.1 ; linear velocity x + (1,0) 0.1 ; linear velocity y + (2,0) 0.1 ; linear velocity z + (3,0) 0.3 ; euler angle velocity z + (4,0) 0.3 ; euler angle velocity y + (5,0) 0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) 0.1 ; forward velocity + (1,0) 0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs + arm + { + (0,0) 15.0 ; arm_1 [rad/s] + (1,0) 15.0 ; arm_2 [rad/s] + (2,0) 15.0 ; arm_3 [rad/s] + (3,0) 15.0 ; arm_4 [rad/s] + (4,0) 15.0 ; arm_5 [rad/s] + (5,0) 15.0 ; arm_6 [rad/s] + } + } +} diff --git a/resources/config/realman/realman.info b/resources/config/realman/realman.info new file mode 100644 index 0000000..85b775f --- /dev/null +++ b/resources/config/realman/realman.info @@ -0,0 +1,313 @@ +; robot model meta-information +model_information { + manipulatorModelType 0 // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator + + ; motion joints in the URDF to consider fixed + removeJoints { + } + + ; base frame of the robot (from URDF) + baseFrame "root" + ; end-effector frame of the robot (from URDF) + eeFrame "link_6" +} + +model_settings +{ + usePreComputation true + recompileLibraries true +} + +; DDP settings +ddp +{ + algorithm SLQ + + nThreads 3 + threadPriority 50 + + maxNumIterations 1 + minRelCost 0.1 + constraintTolerance 1e-3 + + displayInfo false + displayShortSummary false + checkNumericalStability false + debugPrintRollout false + debugCaching false + + AbsTolODE 1e-5 + RelTolODE 1e-3 + maxNumStepsPerSecond 100000 + timeStep 1e-3 + backwardPassIntegratorType ODE45 + + constraintPenaltyInitialValue 20.0 + constraintPenaltyIncreaseRate 2.0 + + preComputeRiccatiTerms true + + useFeedbackPolicy false + + strategy LINE_SEARCH + lineSearch + { + minStepLength 1e-2 + maxStepLength 1.0 + hessianCorrectionStrategy DIAGONAL_SHIFT + hessianCorrectionMultiple 1e-3 + } +} + +; Rollout settings +rollout +{ + AbsTolODE 1e-5 + RelTolODE 1e-3 + timeStep 1e-2 + integratorType ODE45 + maxNumStepsPerSecond 100000 + checkNumericalStability false +} + +; MPC settings +mpc +{ + timeHorizon 1.0 ; [s] + solutionTimeWindow 0.2 ; [s] + coldStart false + + debugPrint false + + mpcDesiredFrequency 100 ; [Hz] + mrtDesiredFrequency 400 ; [Hz] +} + +; initial state +initialState +{ + ; initial state for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; heading + } + } + + ; initial state for the arm DOFs + arm + { + (0,0) 0.0 ; arm_1 + (1,0) 0.0 ; arm_2 + (2,0) 0.0 ; arm_3 + (3,0) 0.0 ; arm_4 + (4,0) 0.0 ; arm_5 + (5,0) 0.0 ; arm_6 + } +} + +inputCost +{ + ; control weight matrix + R + { + ; input costs for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + scaling 1e-2 + + (0,0) 5.0 ; position x + (1,1) 5.0 ; position y + (2,2) 5.0 ; position z + (3,3) 5.0 ; euler angle z + (4,4) 5.0 ; euler angle y + (5,5) 5.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + scaling 1e-2 + + (0,0) 5.0 ; forward velocity + (1,1) 5.0 ; turning velocity + } + } + + ; input costs for the arm DOFs + arm + { + scaling 1e-2 + + (0,0) 1.0 ; arm_1 velocity + (1,1) 1.0 ; arm_2 velocity + (2,2) 1.0 ; arm_3 velocity + (3,3) 1.0 ; arm_4 velocity + (4,4) 1.0 ; arm_5 velocity + (5,5) 1.0 ; arm_6 velocity + } + } +} + +endEffector +{ + ; end effector quadratic penalty scaling + muPosition 10.0 + muOrientation 5.0 +} + +finalEndEffector +{ + muPosition 10.0 + muOrientation 5.0 +} + +selfCollision +{ + ; activate self-collision constraint + activate false + + ; TODO: Replace the collision meshes of the arm with primitive shapes. +} + +; Only applied for arm joints: limits parsed from URDF +jointPositionLimits +{ + ; activate constraint + activate true + + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 +} + +jointVelocityLimits +{ + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 + + lowerBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) -0.1 ; linear velocity x + (1,0) -0.1 ; linear velocity y + (2,0) -0.1 ; linear velocity z + (3,0) -0.3 ; euler angle velocity z + (4,0) -0.3 ; euler angle velocity y + (5,0) -0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) -0.1 ; forward velocity + (1,0) -0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs + arm + { + (0,0) -15.0 ; arm_1 [rad/s] + (1,0) -15.0 ; arm_2 [rad/s] + (2,0) -15.0 ; arm_3 [rad/s] + (3,0) -15.0 ; arm_4 [rad/s] + (4,0) -15.0 ; arm_5 [rad/s] + (5,0) -15.0 ; arm_6 [rad/s] + } + } + + upperBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.1 ; linear velocity x + (1,0) 0.1 ; linear velocity y + (2,0) 0.1 ; linear velocity z + (3,0) 0.3 ; euler angle velocity z + (4,0) 0.3 ; euler angle velocity y + (5,0) 0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) 0.1 ; forward velocity + (1,0) 0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs + arm + { + (0,0) 3.14 ; arm_1 [rad/s] + (1,0) 3.14 ; arm_2 [rad/s] + (2,0) 3.92 ; arm_3 [rad/s] + (3,0) 3.92 ; arm_4 [rad/s] + (4,0) 3.92 ; arm_5 [rad/s] + (5,0) 3.92 ; arm_6 [rad/s] + } + } +} diff --git a/resources/realman/meshes/base_link.STL b/resources/realman/meshes/base_link.STL new file mode 100644 index 0000000..0889070 Binary files /dev/null and b/resources/realman/meshes/base_link.STL differ diff --git a/resources/realman/meshes/link1.STL b/resources/realman/meshes/link1.STL new file mode 100644 index 0000000..9e488ed Binary files /dev/null and b/resources/realman/meshes/link1.STL differ diff --git a/resources/realman/meshes/link2.STL b/resources/realman/meshes/link2.STL new file mode 100644 index 0000000..7c40979 Binary files /dev/null and b/resources/realman/meshes/link2.STL differ diff --git a/resources/realman/meshes/link3.STL b/resources/realman/meshes/link3.STL new file mode 100644 index 0000000..de9e6b6 Binary files /dev/null and b/resources/realman/meshes/link3.STL differ diff --git a/resources/realman/meshes/link4.STL b/resources/realman/meshes/link4.STL new file mode 100644 index 0000000..80ce2f4 Binary files /dev/null and b/resources/realman/meshes/link4.STL differ diff --git a/resources/realman/meshes/link5.STL b/resources/realman/meshes/link5.STL new file mode 100644 index 0000000..d40ecc3 Binary files /dev/null and b/resources/realman/meshes/link5.STL differ diff --git a/resources/realman/meshes/link6.STL b/resources/realman/meshes/link6.STL new file mode 100644 index 0000000..b67bf2b Binary files /dev/null and b/resources/realman/meshes/link6.STL differ diff --git a/resources/realman/urdf/rm_65.urdf b/resources/realman/urdf/rm_65.urdf new file mode 100644 index 0000000..b4b923b --- /dev/null +++ b/resources/realman/urdf/rm_65.urdf @@ -0,0 +1,403 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/realman/urdf/rm_65_fixed.urdf b/resources/realman/urdf/rm_65_fixed.urdf new file mode 100644 index 0000000..b240a2d --- /dev/null +++ b/resources/realman/urdf/rm_65_fixed.urdf @@ -0,0 +1,399 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/realman/urdf/rm_65_new.urdf b/resources/realman/urdf/rm_65_new.urdf new file mode 100644 index 0000000..0b7adea --- /dev/null +++ b/resources/realman/urdf/rm_65_new.urdf @@ -0,0 +1,394 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/realman/urdf/rm_robot.urdf b/resources/realman/urdf/rm_robot.urdf new file mode 100644 index 0000000..c19b430 --- /dev/null +++ b/resources/realman/urdf/rm_robot.urdf @@ -0,0 +1,315 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + /arm + gazebo_ros_control/DefaultRobotHWSim + true + + + + diff --git a/resources/realman/urdf/test.urdf b/resources/realman/urdf/test.urdf new file mode 100644 index 0000000..bf005d3 --- /dev/null +++ b/resources/realman/urdf/test.urdf @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rviz/mobile_manipulator.rviz b/rviz/mobile_manipulator.rviz new file mode 100644 index 0000000..2de59a8 --- /dev/null +++ b/rviz/mobile_manipulator.rviz @@ -0,0 +1,272 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + ARM: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + All Links Enabled: true + ELBOW: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FOREARM: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + SHOULDER: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WRIST_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WRIST_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + ARM: + Value: true + All Enabled: true + ELBOW: + Value: true + FOREARM: + Value: true + SHOULDER: + Value: true + WRIST_1: + Value: true + WRIST_2: + Value: true + arm_base: + Value: true + arm_base_inertia: + Value: true + base: + Value: true + command: + Value: true + world: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base: + arm_base: + SHOULDER: + ARM: + ELBOW: + FOREARM: + WRIST_1: + WRIST_2: + {} + arm_base_inertia: + {} + command: + {} + Update Interval: 0 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /mobile_manipulator/optimizedStateTrajectory + Name: MarkerArray + Namespaces: + Base Trajectory: true + EE Trajectory: true + Queue Size: 100 + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: /mobile_manipulator/optimizedPoseTrajectory + Unreliable: false + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /simple_marker/update + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /distance_markers + Name: MarkerArray + Namespaces: + 1 - 4: true + 1 - 6: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 186; 189; 182 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.996953964233398 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4003985524177551 + Target Frame: + Yaw: 0.44039851427078247 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 720 + Y: 280 diff --git a/rviz/mobile_manipulator_distance.rviz b/rviz/mobile_manipulator_distance.rviz new file mode 100644 index 0000000..79a6190 --- /dev/null +++ b/rviz/mobile_manipulator_distance.rviz @@ -0,0 +1,245 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 732 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/RobotModel + Collision Enabled: true + Enabled: true + Links: + ARM: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + All Links Enabled: true + ELBOW: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FOREARM: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + SHOULDER: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WRIST_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WRIST_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + ARM: + Value: true + All Enabled: true + ELBOW: + Value: true + FOREARM: + Value: true + SHOULDER: + Value: true + WRIST_1: + Value: true + WRIST_2: + Value: true + arm_base: + Value: true + arm_base_inertia: + Value: true + base: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base: + arm_base: + SHOULDER: + ARM: + ELBOW: + FOREARM: + WRIST_1: + WRIST_2: + {} + arm_base_inertia: + {} + Update Interval: 0 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /distance_markers + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: /mobile_manipulator/optimizedPoseTrajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.469738483428955 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.4907682240009308 + Y: 0.06928219646215439 + Z: 0.17322030663490295 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.33039841055870056 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.3103978633880615 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1029 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000022900000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000367000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004dc0000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1869 + X: 671 + Y: 87 diff --git a/script/transport.py b/script/transport.py new file mode 100755 index 0000000..dfae14d --- /dev/null +++ b/script/transport.py @@ -0,0 +1,31 @@ +import rospy +from geometry_msgs.msg import PoseStamped +from ocs2_msgs.msg import mpc_target_trajectories, mpc_state, mpc_input +class TransNode: + def __init__(self): + self.pose_sub = rospy.Subscriber("right_arm_tf", PoseStamped, self.sub_callback, queue_size=1, buff_size=2048) + self.target_pub = rospy.Publisher("/mobile_manipulator_mpc_target", mpc_target_trajectories, queue_size=1) + + def sub_callback(self, msg): + # print(msg) + position = msg.pose.position + orientation = msg.pose.orientation + # print(position, orientation) + combine_data = [position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w] + # print(combine_data) + target = mpc_target_trajectories() + state = mpc_state() + state.value = combine_data + input = mpc_input() + input.value = [0, 0, 0, 0, 0, 0] + target.timeTrajectory = [0] + target.stateTrajectory = [state] + target.inputTrajectory = [input] + print(target) + self.target_pub.publish(target) + +if __name__ == "__main__": + rospy.init_node("transNode") + rospy.loginfo("start trans node...") + node = TransNode() + rospy.spin() \ No newline at end of file diff --git a/src/MobileManipulatorDistanceVisualization.cpp b/src/MobileManipulatorDistanceVisualization.cpp new file mode 100644 index 0000000..570a426 --- /dev/null +++ b/src/MobileManipulatorDistanceVisualization.cpp @@ -0,0 +1,130 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// needs to be included before boost +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +using namespace ocs2; +using namespace mobile_manipulator; + +std::unique_ptr pInterface; +std::shared_ptr gInterface; +std::unique_ptr vInterface; + +sensor_msgs::JointState lastMsg; + +std::unique_ptr pub; + +void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { + if (lastMsg.position == msg->position) { + return; + } + lastMsg.position = msg->position; + + Eigen::VectorXd q(9); + q(0) = q(1) = q(2) = 0.0; + for (size_t i = 3; i < 9; ++i) { + q(i) = lastMsg.position[i - 3]; + } + + vInterface->publishDistances(q); +} + +int main(int argc, char** argv) { + // Initialize ros node + ros::init(argc, argv, "distance_visualization"); + ros::NodeHandle nodeHandle; + // Get ROS parameters + std::string urdfPath, taskFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfPath); + + // read the task file + boost::property_tree::ptree pt; + boost::property_tree::read_info(taskFile, pt); + // read manipulator type + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + // read the joints to make fixed + std::vector removeJointNames; + loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames, true); + // read the frame names + std::string baseFrame; + loadData::loadPtreeValue(pt, baseFrame, "model_information.baseFrame", false); + + // create pinocchio interface + pInterface.reset(new PinocchioInterface(::ocs2::mobile_manipulator::createPinocchioInterface(urdfPath, modelType))); + + std::cerr << "\n #### Model Information:"; + std::cerr << "\n #### =============================================================================\n"; + std::cerr << "\n #### model_information.manipulatorModelType: " << static_cast(modelType); + std::cerr << "\n #### model_information.removeJoints: "; + for (const auto& name : removeJointNames) { + std::cerr << "\"" << name << "\" "; + } + std::cerr << "\n #### model_information.baseFrame: \"" << baseFrame << "\""; + + std::vector> selfCollisionObjectPairs; + std::vector> selfCollisionLinkPairs; + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", selfCollisionObjectPairs); + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionLinkPairs", selfCollisionLinkPairs); + for (const auto& element : selfCollisionObjectPairs) { + std::cerr << "[" << element.first << ", " << element.second << "]; "; + } + std::cerr << std::endl; + std::cerr << "Loaded collision link pairs: "; + for (const auto& element : selfCollisionLinkPairs) { + std::cerr << "[" << element.first << ", " << element.second << "]; "; + } + std::cerr << std::endl; + + gInterface.reset(new PinocchioGeometryInterface(*pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); + + vInterface.reset(new GeometryInterfaceVisualization(*pInterface, *gInterface, nodeHandle, baseFrame)); + + ros::Subscriber sub = nodeHandle.subscribe("joint_states", 1, &jointStateCallback); + + ros::spin(); + + return 0; +} diff --git a/src/MobileManipulatorDummyMRT.cpp b/src/MobileManipulatorDummyMRT.cpp new file mode 100644 index 0000000..b472ae5 --- /dev/null +++ b/src/MobileManipulatorDummyMRT.cpp @@ -0,0 +1,91 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include + +#include + +#include +#include +#include + +#include +#include + +using namespace ocs2; +using namespace mobile_manipulator; + +int main(int argc, char** argv) { + const std::string robotName = "mobile_manipulator"; + + // Initialize ros node + ros::init(argc, argv, robotName + "_mrt"); + ros::NodeHandle nodeHandle; + // Get node parameters + std::string taskFile, libFolder, urdfFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/libFolder", libFolder); + nodeHandle.getParam("/urdfFile", urdfFile); + std::cerr << "Loading task file: " << taskFile << std::endl; + std::cerr << "Loading library folder: " << libFolder << std::endl; + std::cerr << "Loading urdf file: " << urdfFile << std::endl; + // Robot Interface + mobile_manipulator::MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); + + // MRT + MRT_ROS_Interface mrt(robotName); + mrt.initRollout(&interface.getRollout()); + mrt.launchNodes(nodeHandle); + + // Visualization + auto dummyVisualization = std::make_shared(nodeHandle, interface); + + // Dummy MRT + MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); + dummy.subscribeObservers({dummyVisualization}); + + // initial state + SystemObservation initObservation; + initObservation.state = interface.getInitialState(); + initObservation.input.setZero(interface.getManipulatorModelInfo().inputDim); + initObservation.time = 0.0; + + // initial command + vector_t initTarget(7); + initTarget.head(3) << 1, 0, 1; + initTarget.tail(4) << Eigen::Quaternion(1, 0, 0, 0).coeffs(); + const vector_t zeroInput = vector_t::Zero(interface.getManipulatorModelInfo().inputDim); + const TargetTrajectories initTargetTrajectories({initObservation.time}, {initTarget}, {zeroInput}); + + // Run dummy (loops while ros is ok) + dummy.run(initObservation, initTargetTrajectories); + + // Successful exit + return 0; +} diff --git a/src/MobileManipulatorDummyVisualization.cpp b/src/MobileManipulatorDummyVisualization.cpp new file mode 100644 index 0000000..9a1d6dc --- /dev/null +++ b/src/MobileManipulatorDummyVisualization.cpp @@ -0,0 +1,242 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ocs2 { +namespace mobile_manipulator { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { + for (; firstIt != lastIt; ++firstIt) { + firstIt->header = header; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { + for (; firstIt != lastIt; ++firstIt) { + firstIt->id = startId++; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { + // load a kdl-tree from the urdf robot description and initialize the robot state publisher + const std::string urdfName = "robot_description"; + urdf::Model model; + if (!model.initParam(urdfName)) { + ROS_ERROR("URDF model load was NOT successful"); + } + KDL::Tree tree; + if (!kdl_parser::treeFromUrdfModel(model, tree)) { + ROS_ERROR("Failed to extract kdl tree from xml robot description"); + } + + robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); + robotStatePublisherPtr_->publishFixedTransforms(true); + + stateOptimizedPublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedPoseTrajectory", 1); + // Get ROS parameter + std::string urdfFile, taskFile; + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/taskFile", taskFile); + // read manipulator type + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + // read the joints to make fixed + loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames_, false); + // read if self-collision checking active + boost::property_tree::ptree pt; + boost::property_tree::read_info(taskFile, pt); + bool activateSelfCollision = true; + loadData::loadPtreeValue(pt, activateSelfCollision, "selfCollision.activate", true); + // create pinocchio interface + PinocchioInterface pinocchioInterface(mobile_manipulator::createPinocchioInterface(urdfFile, modelType, removeJointNames_)); + // activate markers for self-collision visualization + if (activateSelfCollision) { + std::vector> collisionObjectPairs; + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", collisionObjectPairs, true); + PinocchioGeometryInterface geomInterface(pinocchioInterface, collisionObjectPairs); + // set geometry visualization markers + geometryVisualization_.reset(new GeometryInterfaceVisualization(std::move(pinocchioInterface), geomInterface, nodeHandle)); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MobileManipulatorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, + const CommandData& command) { + const ros::Time timeStamp = ros::Time::now(); + + publishObservation(timeStamp, observation); + publishTargetTrajectories(timeStamp, command.mpcTargetTrajectories_); + publishOptimizedTrajectory(timeStamp, policy); + if (geometryVisualization_ != nullptr) { + geometryVisualization_->publishDistances(observation.state); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& timeStamp, const SystemObservation& observation) { + // publish world -> base transform + const auto r_world_base = getBasePosition(observation.state, modelInfo_); + const Eigen::Quaternion q_world_base = getBaseOrientation(observation.state, modelInfo_); + + geometry_msgs::TransformStamped base_tf; + base_tf.header.stamp = timeStamp; + base_tf.header.frame_id = "world"; + base_tf.child_frame_id = modelInfo_.baseFrame; + base_tf.transform.translation = ros_msg_helpers::getVectorMsg(r_world_base); + base_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(q_world_base); + tfBroadcaster_.sendTransform(base_tf); + + // publish joints transforms + const auto j_arm = getArmJointAngles(observation.state, modelInfo_); + std::map jointPositions; + for (size_t i = 0; i < modelInfo_.dofNames.size(); i++) { + jointPositions[modelInfo_.dofNames[i]] = j_arm(i); + } + for (const auto& name : removeJointNames_) { + jointPositions[name] = 0.0; + } + robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MobileManipulatorDummyVisualization::publishTargetTrajectories(const ros::Time& timeStamp, + const TargetTrajectories& targetTrajectories) { + // publish command transform + const Eigen::Vector3d eeDesiredPosition = targetTrajectories.stateTrajectory.back().head(3); + Eigen::Quaterniond eeDesiredOrientation; + eeDesiredOrientation.coeffs() = targetTrajectories.stateTrajectory.back().tail(4); + geometry_msgs::TransformStamped command_tf; + command_tf.header.stamp = timeStamp; + command_tf.header.frame_id = "world"; + command_tf.child_frame_id = "command"; + command_tf.transform.translation = ros_msg_helpers::getVectorMsg(eeDesiredPosition); + command_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); + tfBroadcaster_.sendTransform(command_tf); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy) { + const scalar_t TRAJECTORYLINEWIDTH = 0.005; + const std::array red{0.6350, 0.0780, 0.1840}; + const std::array blue{0, 0.4470, 0.7410}; + const auto& mpcStateTrajectory = policy.stateTrajectory_; + + visualization_msgs::MarkerArray markerArray; + + // Base trajectory + std::vector baseTrajectory; + baseTrajectory.reserve(mpcStateTrajectory.size()); + geometry_msgs::PoseArray poseArray; + poseArray.poses.reserve(mpcStateTrajectory.size()); + + // End effector trajectory + const auto& model = pinocchioInterface_.getModel(); + auto& data = pinocchioInterface_.getData(); + + std::vector endEffectorTrajectory; + endEffectorTrajectory.reserve(mpcStateTrajectory.size()); + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const Eigen::VectorXd& state) { + pinocchio::forwardKinematics(model, data, state); + pinocchio::updateFramePlacements(model, data); + const auto eeIndex = model.getBodyId(modelInfo_.eeFrame); + const vector_t eePosition = data.oMf[eeIndex].translation(); + endEffectorTrajectory.push_back(ros_msg_helpers::getPointMsg(eePosition)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(endEffectorTrajectory), blue, TRAJECTORYLINEWIDTH)); + markerArray.markers.back().ns = "EE Trajectory"; + + // Extract base pose from state + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { + // extract from observation + const auto r_world_base = getBasePosition(state, modelInfo_); + const Eigen::Quaternion q_world_base = getBaseOrientation(state, modelInfo_); + + // convert to ros message + geometry_msgs::Pose pose; + pose.position = ros_msg_helpers::getPointMsg(r_world_base); + pose.orientation = ros_msg_helpers::getOrientationMsg(q_world_base); + baseTrajectory.push_back(pose.position); + poseArray.poses.push_back(std::move(pose)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(baseTrajectory), red, TRAJECTORYLINEWIDTH)); + markerArray.markers.back().ns = "Base Trajectory"; + + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ros_msg_helpers::getHeaderMsg("world", timeStamp)); + assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + poseArray.header = ros_msg_helpers::getHeaderMsg("world", timeStamp); + + stateOptimizedPublisher_.publish(markerArray); + stateOptimizedPosePublisher_.publish(poseArray); +} + +} // namespace mobile_manipulator +} // namespace ocs2 diff --git a/src/MobileManipulatorMpcNode.cpp b/src/MobileManipulatorMpcNode.cpp new file mode 100644 index 0000000..64ebb95 --- /dev/null +++ b/src/MobileManipulatorMpcNode.cpp @@ -0,0 +1,74 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include +#include + +#include +#include +#include + +#include + +using namespace ocs2; +using namespace mobile_manipulator; + +int main(int argc, char** argv) { + const std::string robotName = "mobile_manipulator"; + + // Initialize ros node + ros::init(argc, argv, robotName + "_mpc"); + ros::NodeHandle nodeHandle; + // Get node parameters + std::string taskFile, libFolder, urdfFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/libFolder", libFolder); + nodeHandle.getParam("/urdfFile", urdfFile); + std::cerr << "Loading task file: " << taskFile << std::endl; + std::cerr << "Loading library folder: " << libFolder << std::endl; + std::cerr << "Loading urdf file: " << urdfFile << std::endl; + // Robot interface + MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} diff --git a/src/MobileManipulatorTarget.cpp b/src/MobileManipulatorTarget.cpp new file mode 100644 index 0000000..371d169 --- /dev/null +++ b/src/MobileManipulatorTarget.cpp @@ -0,0 +1,60 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include + +using namespace ocs2; + +/** + * Converts the pose of the interactive marker to TargetTrajectories. + */ +TargetTrajectories goalPoseToTargetTrajectories(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation) { + // time trajectory + const scalar_array_t timeTrajectory{observation.time}; + // state trajectory: 3 + 4 for desired position vector and orientation quaternion + const vector_t target = (vector_t(7) << position, orientation.coeffs()).finished(); + const vector_array_t stateTrajectory{target}; + // input trajectory + const vector_array_t inputTrajectory{vector_t::Zero(observation.input.size())}; + + return {timeTrajectory, stateTrajectory, inputTrajectory}; +} + +int main(int argc, char* argv[]) { + const std::string robotName = "mobile_manipulator"; + ::ros::init(argc, argv, robotName + "_target"); + ::ros::NodeHandle nodeHandle; + + TargetTrajectoriesInteractiveMarker targetPoseCommand(nodeHandle, robotName, &goalPoseToTargetTrajectories); + targetPoseCommand.publishInteractiveMarker(); + + // Successful exit + return 0; +}