first commit
This commit is contained in:
commit
fd8b4d38a2
162
CMakeLists.txt
Normal file
162
CMakeLists.txt
Normal file
@ -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}
|
||||
)
|
||||
@ -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 <robot_state_publisher/robot_state_publisher.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
|
||||
#include <ocs2_ros_interfaces/mrt/DummyObserver.h>
|
||||
|
||||
#include <ocs2_mobile_manipulator/ManipulatorModelInfo.h>
|
||||
#include <ocs2_mobile_manipulator/MobileManipulatorInterface.h>
|
||||
#include <ocs2_self_collision_visualization/GeometryInterfaceVisualization.h>
|
||||
|
||||
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<std::string> removeJointNames_;
|
||||
|
||||
std::unique_ptr<robot_state_publisher::RobotStatePublisher> robotStatePublisherPtr_;
|
||||
tf::TransformBroadcaster tfBroadcaster_;
|
||||
|
||||
ros::Publisher stateOptimizedPublisher_;
|
||||
ros::Publisher stateOptimizedPosePublisher_;
|
||||
|
||||
std::unique_ptr<GeometryInterfaceVisualization> geometryVisualization_;
|
||||
};
|
||||
|
||||
} // namespace mobile_manipulator
|
||||
} // namespace ocs2
|
||||
35
launch/aloha.launch
Normal file
35
launch/aloha.launch
Normal file
@ -0,0 +1,35 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/aloha/wx250.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/aloha/task1.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/aloha" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
|
||||
<node
|
||||
name="master_right_transform_broadcaster"
|
||||
pkg="tf2_ros"
|
||||
type="static_transform_publisher"
|
||||
args="0.145 -0.26525 0.45694 0.3826834 0 0 0.9238795 /world /wx250/base_link" />
|
||||
|
||||
</launch>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
37
launch/include/mobile_manipulator.launch
Normal file
37
launch/include/mobile_manipulator.launch
Normal file
@ -0,0 +1,37 @@
|
||||
<?xml version="1.0" ?>
|
||||
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" />
|
||||
|
||||
<!-- make the files into global parameters -->
|
||||
<param name="taskFile" value="$(arg taskFile)" />
|
||||
<param name="urdfFile" value="$(arg urdfFile)" />
|
||||
<param name="libFolder" value="$(arg libFolder)" />
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/visualize.launch">
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<node if="$(arg debug)" pkg="ocs2_mobile_manipulator_ros" type="mobile_manipulator_mpc_node" name="mobile_manipulator_mpc_node"
|
||||
output="screen" launch-prefix="gnome-terminal -- gdb -ex run --args" />
|
||||
<node unless="$(arg debug)" pkg="ocs2_mobile_manipulator_ros" type="mobile_manipulator_mpc_node" name="mobile_manipulator_mpc_node"
|
||||
output="screen" launch-prefix="" />
|
||||
|
||||
<node pkg="ocs2_mobile_manipulator_ros" type="mobile_manipulator_dummy_mrt_node" name="mobile_manipulator_dummy_mrt_node"
|
||||
output="screen" launch-prefix="gnome-terminal --" />
|
||||
|
||||
<node if="$(arg rviz)" pkg="ocs2_mobile_manipulator_ros" type="mobile_manipulator_target" name="mobile_manipulator_target"
|
||||
output="screen" launch-prefix="" />
|
||||
</launch>
|
||||
29
launch/include/mobile_manipulator_distance.launch
Normal file
29
launch/include/mobile_manipulator_distance.launch
Normal file
@ -0,0 +1,29 @@
|
||||
<?xml version="1.0" ?>
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" />
|
||||
<!-- Configuration for rviz file -->
|
||||
<arg name="rvizconfig" value="$(find ocs2_mobile_manipulator_ros)/rviz/mobile_manipulator_distance.rviz" />
|
||||
|
||||
<!-- make the files into global parameters -->
|
||||
<param name="urdfFile" value="$(arg urdfFile)" />
|
||||
<param name="taskFile" value="$(arg taskFile)" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/visualize.launch">
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
</include>
|
||||
|
||||
<!-- Open joint_state_publisher for alma_c_description with GUI for debugging -->
|
||||
<node name="mobile_manipulator_joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" output="screen">
|
||||
<param name="rate" value="100" />
|
||||
</node>
|
||||
|
||||
<node pkg="ocs2_mobile_manipulator_ros" type="mobile_manipulator_distance_visualization" name="mobile_manipulator_distance_visualization" output="screen" />
|
||||
|
||||
|
||||
</launch>
|
||||
20
launch/include/visualize.launch
Normal file
20
launch/include/visualize.launch
Normal file
@ -0,0 +1,20 @@
|
||||
<?xml version="1.0" ?>
|
||||
|
||||
<launch>
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" />
|
||||
<!-- Publishes joint state publisher for testing. -->
|
||||
<arg name="test" default="false"/>
|
||||
<!-- Enable rviz visualization. -->
|
||||
<arg name="rviz" default="true"/>
|
||||
<!-- The rviz configuration file to load. -->
|
||||
<arg name="rvizconfig" default="$(find ocs2_mobile_manipulator_ros)/rviz/mobile_manipulator.rviz"/>
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdfFile)"/>
|
||||
<param name="use_gui" value="true"/>
|
||||
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<node if="$(arg test)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
|
||||
<node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" output="screen" />
|
||||
|
||||
</launch>
|
||||
21
launch/manipulator_franka.launch
Normal file
21
launch/manipulator_franka.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/franka/urdf/panda.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/franka/task.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/franka" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_kinova_j2n6.launch
Normal file
21
launch/manipulator_kinova_j2n6.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/kinova/urdf/j2n6s300.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/kinova/task_j2n6.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/kinova/j2n6" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_kinova_j2n7.launch
Normal file
21
launch/manipulator_kinova_j2n7.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/kinova/urdf/j2n7s300.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/kinova/task_j2n7.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/kinova/j2n7" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_mabi_mobile.launch
Normal file
21
launch/manipulator_mabi_mobile.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/mabi_mobile/urdf/mabi_mobile.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/mabi_mobile/task.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/mabi_mobile" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_pr2.launch
Normal file
21
launch/manipulator_pr2.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/pr2/urdf/pr2.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/pr2/task.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/pr2" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_realman.launch
Normal file
21
launch/manipulator_realman.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/realman/urdf/rm_65_new.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/realman/realman.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/realman/rm_65" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
21
launch/manipulator_ridgeback_ur5.launch
Normal file
21
launch/manipulator_ridgeback_ur5.launch
Normal file
@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Enable rviz visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<!-- Set nodes on debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- The URDF model of the robot -->
|
||||
<arg name="urdfFile" value="$(find ocs2_robotic_assets)/resources/mobile_manipulator/ridgeback_ur5/urdf/ridgeback_ur5.urdf" />
|
||||
<!-- The task file for the mpc. -->
|
||||
<arg name="taskFile" value="$(find ocs2_mobile_manipulator)/config/ridgeback_ur5/task.info" />
|
||||
<!-- The library folder to generate CppAD codegen into -->
|
||||
<arg name="libFolder" value="$(find ocs2_mobile_manipulator)/auto_generated/ridgeback_ur5" />
|
||||
|
||||
<include file="$(find ocs2_mobile_manipulator_ros)/launch/include/mobile_manipulator.launch">
|
||||
<arg name="rviz" value="$(arg rviz)" />
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="urdfFile" value="$(arg urdfFile)" />
|
||||
<arg name="taskFile" value="$(arg taskFile)" />
|
||||
<arg name="libFolder" value="$(arg libFolder)" />
|
||||
</include>
|
||||
</launch>
|
||||
36
package.xml
Normal file
36
package.xml
Normal file
@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ocs2_mobile_manipulator_ros</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The ocs2_mobile_manipulator_ros package</description>
|
||||
|
||||
<maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
|
||||
<maintainer email="mspieler@ethz.ch">Michael Spieler</maintainer>
|
||||
<maintainer email="fperry@ethz.ch">Perry Franklin</maintainer>
|
||||
<maintainer email="mittalma@ethz.ch">Mayank Mittal</maintainer>
|
||||
|
||||
<license>BSD3</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>cmake_clang_tools</build_depend>
|
||||
|
||||
<depend>roslib</depend>
|
||||
<depend>tf</depend>
|
||||
<depend>urdf</depend>
|
||||
<depend>kdl_parser</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>ocs2_core</depend>
|
||||
<depend>ocs2_ddp</depend>
|
||||
<depend>ocs2_mpc</depend>
|
||||
<depend>ocs2_robotic_tools</depend>
|
||||
<depend>ocs2_robotic_assets</depend>
|
||||
<depend>ocs2_ros_interfaces</depend>
|
||||
<depend>ocs2_pinocchio_interface</depend>
|
||||
<depend>ocs2_self_collision</depend>
|
||||
<depend>ocs2_self_collision_visualization</depend>
|
||||
<depend>ocs2_mobile_manipulator</depend>
|
||||
<depend>pinocchio</depend>
|
||||
|
||||
</package>
|
||||
BIN
resources/aloha/meshes/wx250_10_ar_tag.stl
Normal file
BIN
resources/aloha/meshes/wx250_10_ar_tag.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_1_base.stl
Normal file
BIN
resources/aloha/meshes/wx250_1_base.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_2_shoulder.stl
Normal file
BIN
resources/aloha/meshes/wx250_2_shoulder.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_3_upper_arm.stl
Normal file
BIN
resources/aloha/meshes/wx250_3_upper_arm.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_4_forearm.stl
Normal file
BIN
resources/aloha/meshes/wx250_4_forearm.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_5_wrist.stl
Normal file
BIN
resources/aloha/meshes/wx250_5_wrist.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_6_gripper.stl
Normal file
BIN
resources/aloha/meshes/wx250_6_gripper.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_7_gripper_prop.stl
Normal file
BIN
resources/aloha/meshes/wx250_7_gripper_prop.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_8_gripper_bar.stl
Normal file
BIN
resources/aloha/meshes/wx250_8_gripper_bar.stl
Normal file
Binary file not shown.
BIN
resources/aloha/meshes/wx250_9_gripper_finger.stl
Normal file
BIN
resources/aloha/meshes/wx250_9_gripper_finger.stl
Normal file
Binary file not shown.
399
resources/aloha/urdf/wx250.urdf
Normal file
399
resources/aloha/urdf/wx250.urdf
Normal file
@ -0,0 +1,399 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from wx250.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="wx250">
|
||||
<material name="interbotix_black">
|
||||
<texture filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/interbotix_black.png"/>
|
||||
</material>
|
||||
<link name="root"/>
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0.7853981 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="wx250/base_link"/>
|
||||
</joint>
|
||||
<link name="wx250/base_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_1_base.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0.45"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_1_base.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.0380446000 0.0006138920 0.0193354000"/>
|
||||
<mass value="0.538736"/>
|
||||
<inertia ixx="0.0021150000" ixy="-0.0000163500" ixz="0.0000006998" iyy="0.0006921000" iyz="0.0000464200" izz="0.0025240000"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="waist" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10" lower="-3.141582653589793" upper="3.141582653589793" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.072"/>
|
||||
<parent link="wx250/base_link"/>
|
||||
<child link="wx250/shoulder_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_waist">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="waist">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="waist_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/shoulder_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 -0.003"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_2_shoulder.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 -0.003"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_2_shoulder.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0000223482 0.0000414609 0.0066287000"/>
|
||||
<mass value="0.480879"/>
|
||||
<inertia ixx="0.0003790000" ixy="0.0000000022" ixz="-0.0000003561" iyy="0.0005557000" iyz="0.0000012245" izz="0.0005889000"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder" type="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="20" lower="-1.8849555921538759" upper="1.9896753472735358" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.03865"/>
|
||||
<parent link="wx250/shoulder_link"/>
|
||||
<child link="wx250/upper_arm_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_shoulder">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="shoulder_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/upper_arm_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_3_upper_arm.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_3_upper_arm.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0171605000 0.0000002725 0.1913230000"/>
|
||||
<mass value="0.430811"/>
|
||||
<inertia ixx="0.0034630000" ixy="-0.0000000001" ixz="-0.0000000002" iyy="0.0035870000" iyz="0.0004272000" izz="0.0004566000"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow" type="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="15" lower="-2.1467549799530254" upper="1.6057029118347832" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0.04975 0 0.25"/>
|
||||
<parent link="wx250/upper_arm_link"/>
|
||||
<child link="wx250/forearm_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_elbow">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="elbow">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="elbow_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/forearm_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_4_forearm.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_4_forearm.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.1534230000 -0.0001179685 -0.0004390000"/>
|
||||
<mass value="0.297673"/>
|
||||
<inertia ixx="0.0021760000" ixy="-0.0000030658" ixz="0.0000000000" iyy="0.0000575700" iyz="0.0000000000" izz="0.0021860000"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_angle" type="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="5" lower="-1.7453292519943295" upper="2.1467549799530254" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0.25 0 0"/>
|
||||
<parent link="wx250/forearm_link"/>
|
||||
<child link="wx250/wrist_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_wrist_angle">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_angle">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_angle_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/wrist_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_5_wrist.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_5_wrist.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0423600000 -0.0000106630 0.0105770000"/>
|
||||
<mass value="0.084957"/>
|
||||
<inertia ixx="0.0000308200" ixy="0.0000000191" ixz="0.0000000023" iyy="0.0000282200" iyz="0.0000025481" izz="0.0000315200"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_rotate" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1" lower="-3.141582653589793" upper="3.141582653589793" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0.065 0 0"/>
|
||||
<parent link="wx250/wrist_link"/>
|
||||
<child link="wx250/gripper_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_wrist_rotate">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_rotate">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_rotate_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/gripper_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.02 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_6_gripper.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.02 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_6_gripper.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0216310000 0.0000002516 0.0114100000"/>
|
||||
<mass value="0.072885"/>
|
||||
<inertia ixx="0.0000253700" ixy="0.0000000000" ixz="0.0000000000" iyy="0.0000183600" iyz="0.0000004340" izz="0.0000167400"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_arm" type="fixed">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.043 0 0"/>
|
||||
<parent link="wx250/gripper_link"/>
|
||||
<child link="wx250/ee_arm_link"/>
|
||||
</joint>
|
||||
<link name="wx250/ee_arm_link">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="gripper" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1" velocity="3.141592653589793"/>
|
||||
<origin rpy="0 0 0" xyz="0.0055 0 0"/>
|
||||
<parent link="wx250/ee_arm_link"/>
|
||||
<child link="wx250/gripper_prop_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_gripper">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/gripper_prop_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.0685 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_7_gripper_prop.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.0685 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_7_gripper_prop.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0008460000 -0.0000016817 0.0000420000"/>
|
||||
<mass value="0.00434"/>
|
||||
<inertia ixx="0.0000005923" ixy="0.0000000000" ixz="0.0000003195" iyy="0.0000011156" iyz="-0.0000000004" izz="0.0000005743"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- If the AR tag is being used, then add the AR tag mount -->
|
||||
<!-- If the gripper bar is being used, then also add the gripper bar -->
|
||||
<joint name="gripper_bar" type="fixed">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="wx250/ee_arm_link"/>
|
||||
<child link="wx250/gripper_bar_link"/>
|
||||
</joint>
|
||||
<link name="wx250/gripper_bar_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.063 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_8_gripper_bar.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="-0.063 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_8_gripper_bar.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 1.5707963267948966" xyz="0.0096870000 0.0000008177 0.0049620000"/>
|
||||
<mass value="0.034199"/>
|
||||
<inertia ixx="0.0000074125" ixy="-0.0000000008" ixz="-0.0000000006" iyy="0.0000284300" iyz="-0.0000013889" izz="0.0000286000"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_bar" type="fixed">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.023 0 0"/>
|
||||
<parent link="wx250/gripper_bar_link"/>
|
||||
<child link="wx250/fingers_link"/>
|
||||
</joint>
|
||||
<link name="wx250/fingers_link">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- If the gripper fingers are being used, add those as well -->
|
||||
<joint name="left_finger" type="prismatic">
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="5" lower="0.015" upper="0.037" velocity="1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="wx250/fingers_link"/>
|
||||
<child link="wx250/left_finger_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
</joint>
|
||||
<transmission name="trans_left_finger">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="left_finger">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="left_finger_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/left_finger_link">
|
||||
<visual>
|
||||
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0.005 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_9_gripper_finger.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0.005 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_9_gripper_finger.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="3.141592653589793 3.141592653589793 1.5707963267948966" xyz="0.0138160000 0.0000000000 0.0000000000"/>
|
||||
<mass value="0.016246"/>
|
||||
<inertia ixx="0.0000047310" ixy="-0.0000004560" ixz="0.0000000000" iyy="0.0000015506" iyz="0.0000000000" izz="0.0000037467"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_finger" type="prismatic">
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="5" lower="-0.037" upper="-0.015" velocity="1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="wx250/fingers_link"/>
|
||||
<child link="wx250/right_finger_link"/>
|
||||
<dynamics friction="0.1"/>
|
||||
<mimic joint="left_finger" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<transmission name="trans_right_finger">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="right_finger">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="right_finger_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="wx250/right_finger_link">
|
||||
<visual>
|
||||
<origin rpy="0 3.141592653589793 0" xyz="0 -0.005 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_9_gripper_finger.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="interbotix_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 3.141592653589793 0" xyz="0 -0.005 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/interbotix_xsarm_descriptions/meshes/wx250_meshes/wx250_9_gripper_finger.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 3.141592653589793 1.5707963267948966" xyz="0.0138160000 0.0000000000 0.0000000000"/>
|
||||
<mass value="0.016246"/>
|
||||
<inertia ixx="0.0000047310" ixy="0.0000004560" ixz="0.0000000000" iyy="0.0000015506" iyz="0.0000000000" izz="0.0000037467"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_gripper" type="fixed">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.027575 0 0"/>
|
||||
<parent link="wx250/fingers_link"/>
|
||||
<child link="wx250/ee_gripper_link"/>
|
||||
</joint>
|
||||
<link name="wx250/ee_gripper_link">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
321
resources/config/aloha/aloha.info
Normal file
321
resources/config/aloha/aloha.info
Normal file
@ -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]
|
||||
}
|
||||
}
|
||||
}
|
||||
313
resources/config/realman/realman.info
Normal file
313
resources/config/realman/realman.info
Normal file
@ -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]
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
resources/realman/meshes/base_link.STL
Normal file
BIN
resources/realman/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link1.STL
Normal file
BIN
resources/realman/meshes/link1.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link2.STL
Normal file
BIN
resources/realman/meshes/link2.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link3.STL
Normal file
BIN
resources/realman/meshes/link3.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link4.STL
Normal file
BIN
resources/realman/meshes/link4.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link5.STL
Normal file
BIN
resources/realman/meshes/link5.STL
Normal file
Binary file not shown.
BIN
resources/realman/meshes/link6.STL
Normal file
BIN
resources/realman/meshes/link6.STL
Normal file
Binary file not shown.
403
resources/realman/urdf/rm_65.urdf
Normal file
403
resources/realman/urdf/rm_65.urdf
Normal file
@ -0,0 +1,403 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot
|
||||
name="rm_65_description">
|
||||
<link name="root"/>
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.841070778135659" />
|
||||
<inertia
|
||||
ixx="0.0017261110801622"
|
||||
ixy="2.52746264980217E-06"
|
||||
ixz="-3.67690303614961E-05"
|
||||
iyy="0.00170987405835604"
|
||||
iyz="1.67996364994424E-06"
|
||||
izz="0.000904023422915791" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<!-- <joint name="base_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="Link1"/>
|
||||
</joint> -->
|
||||
<link
|
||||
name="Link1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.593563443690403" />
|
||||
<inertia
|
||||
ixx="0.00126614120341847"
|
||||
ixy="-1.294980943835E-08"
|
||||
ixz="-9.80120923066996E-09"
|
||||
iyy="0.00118168178300364"
|
||||
iyz="-0.00021121727444415"
|
||||
izz="0.00056135241627747" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint1"
|
||||
type="revolute">
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="Link1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.864175046869043" />
|
||||
<inertia
|
||||
ixx="0.00089150298478414"
|
||||
ixy="-2.23268489334765E-08"
|
||||
ixz="0.00156246461035015"
|
||||
iyy="0.00733754343083901"
|
||||
iyz="6.28110889329165E-09"
|
||||
izz="0.00697869103915473" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="1.5708 -1.5708 0" />
|
||||
<parent
|
||||
link="Link1" />
|
||||
<child
|
||||
link="Link2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.268"
|
||||
upper="2.268"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.289633681624654" />
|
||||
<inertia
|
||||
ixx="0.00063737100450158"
|
||||
ixy="-7.05261293649751E-08"
|
||||
ixz="-3.86643272239426E-08"
|
||||
iyy="0.00015648388095025"
|
||||
iyz="-0.00014461035994916"
|
||||
izz="0.000614178164773085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.256 0 0"
|
||||
rpy="0 0 1.5708" />
|
||||
<parent
|
||||
link="Link2" />
|
||||
<child
|
||||
link="Link3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.355"
|
||||
upper="2.355"
|
||||
effort="30"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.239419768320061" />
|
||||
<inertia
|
||||
ixx="0.000285938919722783"
|
||||
ixy="3.07101359163101E-09"
|
||||
ixz="-2.21994118981953E-09"
|
||||
iyy="0.000262727540304212"
|
||||
iyz="4.4236583260078E-05"
|
||||
izz="0.000119888082791859" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="Link3" />
|
||||
<child
|
||||
link="Link4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.218799761431678" />
|
||||
<inertia
|
||||
ixx="0.000350540363914072"
|
||||
ixy="-3.41781619975602E-08"
|
||||
ixz="-1.77056457224373E-08"
|
||||
iyy="0.000104927867487581"
|
||||
iyz="-7.82431228461971E-05"
|
||||
izz="0.000334482418423629" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="Link4" />
|
||||
<child
|
||||
link="Link5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.233"
|
||||
upper="2.233"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0649018034311231" />
|
||||
<inertia
|
||||
ixx="2.02766547502765E-05"
|
||||
ixy="-1.32505200276849E-06"
|
||||
ixz="-2.5845091522508E-08"
|
||||
iyy="1.87986725225022E-05"
|
||||
iyz="3.39471452125439E-09"
|
||||
izz="3.17885459163081E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="Link5" />
|
||||
<child
|
||||
link="Link6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
</robot>
|
||||
399
resources/realman/urdf/rm_65_fixed.urdf
Normal file
399
resources/realman/urdf/rm_65_fixed.urdf
Normal file
@ -0,0 +1,399 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot
|
||||
name="rm_65_description">
|
||||
<link name="root"/>
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.841070778135659" />
|
||||
<inertia
|
||||
ixx="0.0017261110801622"
|
||||
ixy="2.52746264980217E-06"
|
||||
ixz="-3.67690303614961E-05"
|
||||
iyy="0.00170987405835604"
|
||||
iyz="1.67996364994424E-06"
|
||||
izz="0.000904023422915791" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.593563443690403" />
|
||||
<inertia
|
||||
ixx="0.00126614120341847"
|
||||
ixy="-1.294980943835E-08"
|
||||
ixz="-9.80120923066996E-09"
|
||||
iyy="0.00118168178300364"
|
||||
iyz="-0.00021121727444415"
|
||||
izz="0.00056135241627747" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.864175046869043" />
|
||||
<inertia
|
||||
ixx="0.00089150298478414"
|
||||
ixy="-2.23268489334765E-08"
|
||||
ixz="0.00156246461035015"
|
||||
iyy="0.00733754343083901"
|
||||
iyz="6.28110889329165E-09"
|
||||
izz="0.00697869103915473" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="1.5708 -1.5708 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.268"
|
||||
upper="2.268"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.289633681624654" />
|
||||
<inertia
|
||||
ixx="0.00063737100450158"
|
||||
ixy="-7.05261293649751E-08"
|
||||
ixz="-3.86643272239426E-08"
|
||||
iyy="0.00015648388095025"
|
||||
iyz="-0.00014461035994916"
|
||||
izz="0.000614178164773085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.256 0 0"
|
||||
rpy="0 0 1.5708" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.355"
|
||||
upper="2.355"
|
||||
effort="30"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.239419768320061" />
|
||||
<inertia
|
||||
ixx="0.000285938919722783"
|
||||
ixy="3.07101359163101E-09"
|
||||
ixz="-2.21994118981953E-09"
|
||||
iyy="0.000262727540304212"
|
||||
iyz="4.4236583260078E-05"
|
||||
izz="0.000119888082791859" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="10"
|
||||
velocity="3.92" /> <link name="root"/>
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.218799761431678" />
|
||||
<inertia
|
||||
ixx="0.000350540363914072"
|
||||
ixy="-3.41781619975602E-08"
|
||||
ixz="-1.77056457224373E-08"
|
||||
iyy="0.000104927867487581"
|
||||
iyz="-7.82431228461971E-05"
|
||||
izz="0.000334482418423629" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.233"
|
||||
upper="2.233"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0649018034311231" />
|
||||
<inertia
|
||||
ixx="2.02766547502765E-05"
|
||||
ixy="-1.32505200276849E-06"
|
||||
ixz="-2.5845091522508E-08"
|
||||
iyy="1.87986725225022E-05"
|
||||
iyz="3.39471452125439E-09"
|
||||
izz="3.17885459163081E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
</robot>
|
||||
394
resources/realman/urdf/rm_65_new.urdf
Normal file
394
resources/realman/urdf/rm_65_new.urdf
Normal file
@ -0,0 +1,394 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot
|
||||
name="rm_65_description">
|
||||
<link name="root"/>
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.841070778135659" />
|
||||
<inertia
|
||||
ixx="0.0017261110801622"
|
||||
ixy="2.52746264980217E-06"
|
||||
ixz="-3.67690303614961E-05"
|
||||
iyy="0.00170987405835604"
|
||||
iyz="1.67996364994424E-06"
|
||||
izz="0.000904023422915791" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.593563443690403" />
|
||||
<inertia
|
||||
ixx="0.00126614120341847"
|
||||
ixy="-1.294980943835E-08"
|
||||
ixz="-9.80120923066996E-09"
|
||||
iyy="0.00118168178300364"
|
||||
iyz="-0.00021121727444415"
|
||||
izz="0.00056135241627747" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.864175046869043" />
|
||||
<inertia
|
||||
ixx="0.00089150298478414"
|
||||
ixy="-2.23268489334765E-08"
|
||||
ixz="0.00156246461035015"
|
||||
iyy="0.00733754343083901"
|
||||
iyz="6.28110889329165E-09"
|
||||
izz="0.00697869103915473" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="1.5708 -1.5708 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.268"
|
||||
upper="2.268"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.289633681624654" />
|
||||
<inertia
|
||||
ixx="0.00063737100450158"
|
||||
ixy="-7.05261293649751E-08"
|
||||
ixz="-3.86643272239426E-08"
|
||||
iyy="0.00015648388095025"
|
||||
iyz="-0.00014461035994916"
|
||||
izz="0.000614178164773085" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.256 0 0"
|
||||
rpy="0 0 1.5708" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.355"
|
||||
upper="2.355"
|
||||
effort="30"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.239419768320061" />
|
||||
<inertia
|
||||
ixx="0.000285938919722783"
|
||||
ixy="3.07101359163101E-09"
|
||||
ixz="-2.21994118981953E-09"
|
||||
iyy="0.000262727540304212"
|
||||
iyz="4.4236583260078E-05"
|
||||
izz="0.000119888082791859" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.218799761431678" />
|
||||
<inertia
|
||||
ixx="0.000350540363914072"
|
||||
ixy="-3.41781619975602E-08"
|
||||
ixz="-1.77056457224373E-08"
|
||||
iyy="0.000104927867487581"
|
||||
iyz="-7.82431228461971E-05"
|
||||
izz="0.000334482418423629" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.233"
|
||||
upper="2.233"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0649018034311231" />
|
||||
<inertia
|
||||
ixx="2.02766547502765E-05"
|
||||
ixy="-1.32505200276849E-06"
|
||||
ixz="-2.5845091522508E-08"
|
||||
iyy="1.87986725225022E-05"
|
||||
iyz="3.39471452125439E-09"
|
||||
izz="3.17885459163081E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.92" />
|
||||
</joint>
|
||||
</robot>
|
||||
315
resources/realman/urdf/rm_robot.urdf
Normal file
315
resources/realman/urdf/rm_robot.urdf
Normal file
@ -0,0 +1,315 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from rm_65.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="rm_65">
|
||||
<link name="dummy">
|
||||
</link>
|
||||
<!-- /////////////////////////////////////// bottom_joint ////////////////////////////////////////// -->
|
||||
<joint name="bottom_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="bottom_link"/>
|
||||
</joint>
|
||||
<link name="bottom_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 -0.02"/>
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.02"/>
|
||||
</geometry>
|
||||
<material name="Brown"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz=" 0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.02"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="500"/>
|
||||
<inertia ixx="41.68333333333333" ixy="0" ixz="0" iyy="83.33333333333333" iyz="0" izz="41.68333333333333"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"/>
|
||||
<mass value="0.841070778135659"/>
|
||||
<inertia ixx="0.0017261110801622" ixy="2.52746264980217E-06" ixz="-3.67690303614961E-05" iyy="0.00170987405835604" iyz="1.67996364994424E-06" izz="0.000904023422915791"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="base_link">
|
||||
<mu1 value="200000.0"/>
|
||||
<mu2 value="200000.0"/>
|
||||
<kp value="100000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
<joint name="dummy_joint" type="fixed">
|
||||
<parent link="dummy"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link name="Link1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"/>
|
||||
<mass value="0.593563443690403"/>
|
||||
<inertia ixx="0.00126614120341847" ixy="-1.294980943835E-08" ixz="-9.80120923066996E-09" iyy="0.00118168178300364" iyz="-0.00021121727444415" izz="0.00056135241627747"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2405"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="Link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="60" lower="-3.1" upper="3.1" velocity="3.14"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<link name="Link2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"/>
|
||||
<mass value="0.864175046869043"/>
|
||||
<inertia ixx="0.00089150298478414" ixy="-2.23268489334765E-08" ixz="0.00156246461035015" iyy="0.00733754343083901" iyz="6.28110889329165E-09" izz="0.00697869103915473"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link2.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint2" type="revolute">
|
||||
<origin rpy="1.5708 -1.5708 0" xyz="0 0 0"/>
|
||||
<parent link="Link1"/>
|
||||
<child link="Link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="60" lower="-2.268" upper="2.268" velocity="3.14"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<link name="Link3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"/>
|
||||
<mass value="0.289633681624654"/>
|
||||
<inertia ixx="0.00063737100450158" ixy="-7.05261293649751E-08" ixz="-3.86643272239426E-08" iyy="0.00015648388095025" iyz="-0.00014461035994916" izz="0.000614178164773085"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link3.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint3" type="revolute">
|
||||
<origin rpy="0 0 1.5708" xyz="0.256 0 0"/>
|
||||
<parent link="Link2"/>
|
||||
<child link="Link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="30" lower="-2.355" upper="2.355" velocity="3.92"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<link name="Link4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"/>
|
||||
<mass value="0.239419768320061"/>
|
||||
<inertia ixx="0.000285938919722783" ixy="3.07101359163101E-09" ixz="-2.21994118981953E-09" iyy="0.000262727540304212" iyz="4.4236583260078E-05" izz="0.000119888082791859"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link4.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint4" type="revolute">
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.21 0"/>
|
||||
<parent link="Link3"/>
|
||||
<child link="Link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10" lower="-3.1" upper="3.1" velocity="3.92"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<link name="Link5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"/>
|
||||
<mass value="0.218799761431678"/>
|
||||
<inertia ixx="0.000350540363914072" ixy="-3.41781619975602E-08" ixz="-1.77056457224373E-08" iyy="0.000104927867487581" iyz="-7.82431228461971E-05" izz="0.000334482418423629"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link5.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint5" type="revolute">
|
||||
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||
<parent link="Link4"/>
|
||||
<child link="Link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10" lower="-2.233" upper="2.233" velocity="3.92"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<link name="Link6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"/>
|
||||
<mass value="0.0649018034311231"/>
|
||||
<inertia ixx="2.02766547502765E-05" ixy="-1.32505200276849E-06" ixz="-2.5845091522508E-08" iyy="1.87986725225022E-05" iyz="3.39471452125439E-09" izz="3.17885459163081E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link6.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint6" type="revolute">
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.144 0"/>
|
||||
<parent link="Link5"/>
|
||||
<child link="Link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10" lower="-6.28" upper="6.28" velocity="3.92"/>
|
||||
<dynamics damping="50" friction="1"/>
|
||||
</joint>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint2">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint3">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint4">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint5">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="tran1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>/arm</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
<legacyModeNS>true</legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
||||
102
resources/realman/urdf/test.urdf
Normal file
102
resources/realman/urdf/test.urdf
Normal file
@ -0,0 +1,102 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="rm_65_description">
|
||||
<link name="root"/>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.841070778135659" />
|
||||
<inertia
|
||||
ixx="0.0017261110801622"
|
||||
ixy="2.52746264980217E-06"
|
||||
ixz="-3.67690303614961E-05"
|
||||
iyy="0.00170987405835604"
|
||||
iyz="1.67996364994424E-06"
|
||||
izz="0.000904023422915791" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.593563443690403" />
|
||||
<inertia
|
||||
ixx="0.00126614120341847"
|
||||
ixy="-1.294980943835E-08"
|
||||
ixz="-9.80120923066996E-09"
|
||||
iyy="0.00118168178300364"
|
||||
iyz="-0.00021121727444415"
|
||||
izz="0.00056135241627747" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://ocs2_robotic_assets/resources/mobile_manipulator/realman/meshes/link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="root_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="root"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
|
||||
<joint name="joint1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0" />
|
||||
<limit
|
||||
lower="-3.1"
|
||||
upper="3.1"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
</robot>
|
||||
272
rviz/mobile_manipulator.rviz
Normal file
272
rviz/mobile_manipulator.rviz
Normal file
@ -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: <Fixed 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: <Fixed 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
|
||||
245
rviz/mobile_manipulator_distance.rviz
Normal file
245
rviz/mobile_manipulator_distance.rviz
Normal file
@ -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: <Fixed 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: <Fixed 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
|
||||
31
script/transport.py
Executable file
31
script/transport.py
Executable file
@ -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()
|
||||
130
src/MobileManipulatorDistanceVisualization.cpp
Normal file
130
src/MobileManipulatorDistanceVisualization.cpp
Normal file
@ -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 <pinocchio/multibody/geometry.hpp>
|
||||
|
||||
#include <ocs2_core/misc/LoadData.h>
|
||||
#include <ocs2_core/misc/LoadStdVectorOfPair.h>
|
||||
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
|
||||
|
||||
#include <ocs2_self_collision/PinocchioGeometryInterface.h>
|
||||
#include <ocs2_self_collision_visualization/GeometryInterfaceVisualization.h>
|
||||
|
||||
#include <ocs2_mobile_manipulator/FactoryFunctions.h>
|
||||
#include <ocs2_mobile_manipulator/ManipulatorModelInfo.h>
|
||||
#include <ocs2_mobile_manipulator/MobileManipulatorInterface.h>
|
||||
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
using namespace ocs2;
|
||||
using namespace mobile_manipulator;
|
||||
|
||||
std::unique_ptr<PinocchioInterface> pInterface;
|
||||
std::shared_ptr<PinocchioGeometryInterface> gInterface;
|
||||
std::unique_ptr<GeometryInterfaceVisualization> vInterface;
|
||||
|
||||
sensor_msgs::JointState lastMsg;
|
||||
|
||||
std::unique_ptr<ros::Publisher> 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<std::string> removeJointNames;
|
||||
loadData::loadStdVector<std::string>(taskFile, "model_information.removeJoints", removeJointNames, true);
|
||||
// read the frame names
|
||||
std::string baseFrame;
|
||||
loadData::loadPtreeValue<std::string>(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<int>(modelType);
|
||||
std::cerr << "\n #### model_information.removeJoints: ";
|
||||
for (const auto& name : removeJointNames) {
|
||||
std::cerr << "\"" << name << "\" ";
|
||||
}
|
||||
std::cerr << "\n #### model_information.baseFrame: \"" << baseFrame << "\"";
|
||||
|
||||
std::vector<std::pair<size_t, size_t>> selfCollisionObjectPairs;
|
||||
std::vector<std::pair<std::string, std::string>> 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;
|
||||
}
|
||||
91
src/MobileManipulatorDummyMRT.cpp
Normal file
91
src/MobileManipulatorDummyMRT.cpp
Normal file
@ -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 <ocs2_mobile_manipulator/MobileManipulatorInterface.h>
|
||||
|
||||
#include <ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h>
|
||||
|
||||
#include <ocs2_mpc/SystemObservation.h>
|
||||
#include <ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h>
|
||||
#include <ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h>
|
||||
|
||||
#include <ros/init.h>
|
||||
#include <ros/package.h>
|
||||
|
||||
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<mobile_manipulator::MobileManipulatorDummyVisualization>(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<scalar_t>(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;
|
||||
}
|
||||
242
src/MobileManipulatorDummyVisualization.cpp
Normal file
242
src/MobileManipulatorDummyVisualization.cpp
Normal file
@ -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 <pinocchio/fwd.hpp>
|
||||
|
||||
#include <pinocchio/algorithm/frames.hpp>
|
||||
#include <pinocchio/algorithm/kinematics.hpp>
|
||||
|
||||
#include <ros/package.h>
|
||||
#include <tf/tf.h>
|
||||
#include <urdf/model.h>
|
||||
#include <kdl_parser/kdl_parser.hpp>
|
||||
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
|
||||
#include <ocs2_core/misc/LoadData.h>
|
||||
#include <ocs2_core/misc/LoadStdVectorOfPair.h>
|
||||
#include <ocs2_ros_interfaces/common/RosMsgHelpers.h>
|
||||
|
||||
#include <ocs2_mobile_manipulator/AccessHelperFunctions.h>
|
||||
#include <ocs2_mobile_manipulator/FactoryFunctions.h>
|
||||
#include <ocs2_mobile_manipulator/ManipulatorModelInfo.h>
|
||||
#include <ocs2_mobile_manipulator/MobileManipulatorInterface.h>
|
||||
#include <ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h>
|
||||
|
||||
namespace ocs2 {
|
||||
namespace mobile_manipulator {
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
template <typename It>
|
||||
void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) {
|
||||
for (; firstIt != lastIt; ++firstIt) {
|
||||
firstIt->header = header;
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
template <typename It>
|
||||
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<visualization_msgs::MarkerArray>("/mobile_manipulator/optimizedStateTrajectory", 1);
|
||||
stateOptimizedPosePublisher_ = nodeHandle.advertise<geometry_msgs::PoseArray>("/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<std::string>(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<std::pair<size_t, size_t>> 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<scalar_t> 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<std::string, scalar_t> 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<scalar_t, 3> red{0.6350, 0.0780, 0.1840};
|
||||
const std::array<scalar_t, 3> blue{0, 0.4470, 0.7410};
|
||||
const auto& mpcStateTrajectory = policy.stateTrajectory_;
|
||||
|
||||
visualization_msgs::MarkerArray markerArray;
|
||||
|
||||
// Base trajectory
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point> 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<scalar_t> 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
|
||||
74
src/MobileManipulatorMpcNode.cpp
Normal file
74
src/MobileManipulatorMpcNode.cpp
Normal file
@ -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 <ros/init.h>
|
||||
#include <ros/package.h>
|
||||
|
||||
#include <ocs2_ddp/GaussNewtonDDP_MPC.h>
|
||||
#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>
|
||||
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
|
||||
|
||||
#include <ocs2_mobile_manipulator/MobileManipulatorInterface.h>
|
||||
|
||||
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<ocs2::RosReferenceManager>(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;
|
||||
}
|
||||
60
src/MobileManipulatorTarget.cpp
Normal file
60
src/MobileManipulatorTarget.cpp
Normal file
@ -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 <ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h>
|
||||
|
||||
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;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user