Working iisy with ros2_control and actor in same world
This commit is contained in:
Bastian Hofmann
parent
d19bb7e053
commit
c1f72961f3
400
iisz.urdf
Normal file
400
iisz.urdf
Normal file
@ -0,0 +1,400 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from src/iisy_config/urdf/iisy.urdf | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="iisy">
|
||||||
|
<link name="world"/>
|
||||||
|
<link name="base_link"/>
|
||||||
|
<link name="table">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="base_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
|
||||||
|
<mass value="6"/>
|
||||||
|
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="base_top">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 -0.005 0.08"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.15 0.16"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link1_full">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
|
||||||
|
<mass value="4"/>
|
||||||
|
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_bottom">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0.07 0.025"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.14 0.19"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.07 0.025"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
|
||||||
|
<mass value="0.8"/>
|
||||||
|
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="world_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="table"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="table_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 1"/>
|
||||||
|
<parent link="table"/>
|
||||||
|
<child link="base_bottom"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.1205"/>
|
||||||
|
<parent link="base_bottom"/>
|
||||||
|
<child link="base_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
|
||||||
|
<parent link="base_top"/>
|
||||||
|
<child link="link1_full"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.3"/>
|
||||||
|
<parent link="link1_full"/>
|
||||||
|
<child link="link2_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
|
||||||
|
<parent link="link2_bottom"/>
|
||||||
|
<child link="link2_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.0565 0.178003"/>
|
||||||
|
<parent link="link2_top"/>
|
||||||
|
<child link="link3_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
|
||||||
|
<parent link="link3_bottom"/>
|
||||||
|
<child link="link3_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<transmission name="trans_base_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_base_link1_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_link1_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link1_link2_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link1_link2_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_link3_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_link3_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link3_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link3_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<ros2_control name="IgnitionSystem" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>ign_ros2_control/IgnitionSystem</plugin>
|
||||||
|
</hardware>
|
||||||
|
<!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
|
||||||
|
<sensor name="iisy_state">
|
||||||
|
<!-- see KUKA::FRI::LBRState -->
|
||||||
|
<state_interface name="sample_time"/>
|
||||||
|
<state_interface name="time_stamp_sec"/>
|
||||||
|
<state_interface name="time_stamp_nano_sec"/>
|
||||||
|
</sensor>
|
||||||
|
<!-- define joints and command/state interfaces for each joint -->
|
||||||
|
<joint name="base_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<!-- better to use radians as min max first -->
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="ignition-gazebo-joint-state-publisher-system" name="ignition::gazebo::systems::JointStatePublisher"/>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
||||||
|
<parameters>/home/ros/workspace/install/iisy_config/share/iisy_config/config/iisy_controllers.yml</parameters>
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link1_full">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Grey</material>
|
||||||
|
</gazebo>
|
||||||
|
</robot>
|
||||||
36
src/controller_test/CMakeLists.txt
Normal file
36
src/controller_test/CMakeLists.txt
Normal file
@ -0,0 +1,36 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(controller_test)
|
||||||
|
|
||||||
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# find dependencies
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(moveit_ros_planning_interface REQUIRED)
|
||||||
|
find_package(control_msgs REQUIRED)
|
||||||
|
# uncomment the following section in order to fill in
|
||||||
|
# further dependencies manually.
|
||||||
|
# find_package(<dependency> REQUIRED)
|
||||||
|
|
||||||
|
add_executable(move src/test.cpp)
|
||||||
|
set_property(TARGET move PROPERTY CXX_STANDARD 17)
|
||||||
|
ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
|
||||||
|
|
||||||
|
install(TARGETS
|
||||||
|
move
|
||||||
|
DESTINATION lib/${PROJECT_NAME})
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_lint_auto REQUIRED)
|
||||||
|
# the following line skips the linter which checks for copyrights
|
||||||
|
# uncomment the line when a copyright and license is not present in all source files
|
||||||
|
#set(ament_cmake_copyright_FOUND TRUE)
|
||||||
|
# the following line skips cpplint (only works in a git repo)
|
||||||
|
# uncomment the line when this package is not in a git repo
|
||||||
|
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||||
|
ament_lint_auto_find_test_dependencies()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
ament_package()
|
||||||
23
src/controller_test/package.xml
Normal file
23
src/controller_test/package.xml
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>controller_test</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>
|
||||||
|
Test for Moveit on IISY
|
||||||
|
</description>
|
||||||
|
<author email="bastian.hofmann@xitaso.com">bastian</author>
|
||||||
|
<maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>moveit_ros_planning_interface</depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
|
||||||
|
</package>
|
||||||
196
src/controller_test/src/test.cpp
Normal file
196
src/controller_test/src/test.cpp
Normal file
@ -0,0 +1,196 @@
|
|||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <rclcpp_action/rclcpp_action.hpp>
|
||||||
|
|
||||||
|
#include <control_msgs/action/follow_joint_trajectory.hpp>
|
||||||
|
|
||||||
|
std::shared_ptr<rclcpp::Node> node;
|
||||||
|
bool common_goal_accepted = false;
|
||||||
|
rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
|
||||||
|
int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
|
||||||
|
|
||||||
|
void common_goal_response(
|
||||||
|
rclcpp_action::ClientGoalHandle
|
||||||
|
<control_msgs::action::FollowJointTrajectory>::SharedPtr future)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(
|
||||||
|
node->get_logger(), "common_goal_response time: %f",
|
||||||
|
rclcpp::Clock().now().seconds());
|
||||||
|
auto goal_handle = future.get();
|
||||||
|
if (!goal_handle) {
|
||||||
|
common_goal_accepted = false;
|
||||||
|
printf("Goal rejected\n");
|
||||||
|
} else {
|
||||||
|
common_goal_accepted = true;
|
||||||
|
printf("Goal accepted\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void common_result_response(
|
||||||
|
const rclcpp_action::ClientGoalHandle
|
||||||
|
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
|
||||||
|
{
|
||||||
|
printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
|
||||||
|
common_resultcode = result.code;
|
||||||
|
common_action_result_code = result.result->error_code;
|
||||||
|
switch (result.code) {
|
||||||
|
case rclcpp_action::ResultCode::SUCCEEDED:
|
||||||
|
printf("SUCCEEDED result code\n");
|
||||||
|
break;
|
||||||
|
case rclcpp_action::ResultCode::ABORTED:
|
||||||
|
printf("Goal was aborted\n");
|
||||||
|
return;
|
||||||
|
case rclcpp_action::ResultCode::CANCELED:
|
||||||
|
printf("Goal was canceled\n");
|
||||||
|
return;
|
||||||
|
default:
|
||||||
|
printf("Unknown result code\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void common_feedback(
|
||||||
|
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,
|
||||||
|
const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback)
|
||||||
|
{
|
||||||
|
std::cout << "feedback->desired.positions :";
|
||||||
|
for (auto & x : feedback->desired.positions) {
|
||||||
|
std::cout << x << "\t";
|
||||||
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
|
std::cout << "feedback->desired.velocities :";
|
||||||
|
for (auto & x : feedback->desired.velocities) {
|
||||||
|
std::cout << x << "\t";
|
||||||
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
|
||||||
|
node = std::make_shared<rclcpp::Node>("trajectory_test_node");
|
||||||
|
|
||||||
|
std::cout << "node created" << std::endl;
|
||||||
|
|
||||||
|
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
|
||||||
|
action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
|
||||||
|
node->get_node_base_interface(),
|
||||||
|
node->get_node_graph_interface(),
|
||||||
|
node->get_node_logging_interface(),
|
||||||
|
node->get_node_waitables_interface(),
|
||||||
|
"/arm_controller/follow_joint_trajectory");
|
||||||
|
|
||||||
|
bool response =
|
||||||
|
action_client->wait_for_action_server(std::chrono::seconds(1));
|
||||||
|
if (!response) {
|
||||||
|
throw std::runtime_error("could not get action server");
|
||||||
|
}
|
||||||
|
std::cout << "Created action server" << std::endl;
|
||||||
|
|
||||||
|
std::vector<std::string> joint_names = {
|
||||||
|
"base_rot",
|
||||||
|
"base_link1_joint",
|
||||||
|
"link1_link2_joint",
|
||||||
|
"link2_rot",
|
||||||
|
"link2_link3_joint",
|
||||||
|
"link3_rot"
|
||||||
|
};
|
||||||
|
|
||||||
|
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
|
||||||
|
trajectory_msgs::msg::JointTrajectoryPoint point;
|
||||||
|
point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap
|
||||||
|
point.positions.resize(joint_names.size());
|
||||||
|
|
||||||
|
point.positions[0] = 0.0;
|
||||||
|
point.positions[1] = 0.0;
|
||||||
|
point.positions[2] = 0.0;
|
||||||
|
point.positions[3] = 0.0;
|
||||||
|
point.positions[4] = 0.0;
|
||||||
|
point.positions[5] = 0.0;
|
||||||
|
|
||||||
|
trajectory_msgs::msg::JointTrajectoryPoint point2;
|
||||||
|
point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
|
||||||
|
point2.positions.resize(joint_names.size());
|
||||||
|
|
||||||
|
point2.positions[0] = -1.0;
|
||||||
|
point2.positions[1] = 0.0;
|
||||||
|
point2.positions[2] = 0.0;
|
||||||
|
point2.positions[3] = 0.0;
|
||||||
|
point2.positions[4] = 0.0;
|
||||||
|
point2.positions[5] = 0.0;
|
||||||
|
|
||||||
|
trajectory_msgs::msg::JointTrajectoryPoint point3;
|
||||||
|
point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
|
||||||
|
point3.positions.resize(joint_names.size());
|
||||||
|
|
||||||
|
point3.positions[0] = 1.0;
|
||||||
|
point3.positions[1] = 0.0;
|
||||||
|
point3.positions[2] = 0.0;
|
||||||
|
point3.positions[3] = 0.0;
|
||||||
|
point3.positions[4] = 0.0;
|
||||||
|
point3.positions[5] = 0.0;
|
||||||
|
|
||||||
|
trajectory_msgs::msg::JointTrajectoryPoint point4;
|
||||||
|
point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
|
||||||
|
point4.positions.resize(joint_names.size());
|
||||||
|
|
||||||
|
point4.positions[0] = 0.0;
|
||||||
|
point4.positions[1] = 0.0;
|
||||||
|
point4.positions[2] = 0.0;
|
||||||
|
point4.positions[3] = 0.0;
|
||||||
|
point4.positions[4] = 0.0;
|
||||||
|
point4.positions[5] = 0.0;
|
||||||
|
|
||||||
|
points.push_back(point);
|
||||||
|
points.push_back(point2);
|
||||||
|
points.push_back(point3);
|
||||||
|
points.push_back(point4);
|
||||||
|
|
||||||
|
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
|
||||||
|
opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1);
|
||||||
|
opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
|
||||||
|
opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);
|
||||||
|
|
||||||
|
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
|
||||||
|
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
|
||||||
|
goal_msg.trajectory.joint_names = joint_names;
|
||||||
|
goal_msg.trajectory.points = points;
|
||||||
|
|
||||||
|
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
|
||||||
|
|
||||||
|
if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
|
||||||
|
rclcpp::FutureReturnCode::SUCCESS)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
|
||||||
|
|
||||||
|
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
|
||||||
|
goal_handle = goal_handle_future.get();
|
||||||
|
if (!goal_handle) {
|
||||||
|
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
|
||||||
|
|
||||||
|
// Wait for the server to be done with the goal
|
||||||
|
auto result_future = action_client->async_get_result(goal_handle);
|
||||||
|
RCLCPP_INFO(node->get_logger(), "Waiting for result");
|
||||||
|
if (rclcpp::spin_until_future_complete(node, result_future) !=
|
||||||
|
rclcpp::FutureReturnCode::SUCCESS)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "async_send_goal" << std::endl;
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
81
src/ign_world/launch/gazebo_controller_launch.py
Normal file
81
src/ign_world/launch/gazebo_controller_launch.py
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
import os
|
||||||
|
from ament_index_python import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
|
||||||
|
from launch.event_handlers import OnProcessExit
|
||||||
|
from launch_ros.actions import Node, SetParameter
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
import xacro
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
|
||||||
|
load_joint_state_broadcaster = ExecuteProcess(
|
||||||
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
|
||||||
|
output='screen'
|
||||||
|
)
|
||||||
|
|
||||||
|
load_joint_trajectory_controller = ExecuteProcess(
|
||||||
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'arm_controller'],
|
||||||
|
output='screen'
|
||||||
|
)
|
||||||
|
|
||||||
|
ignition_spawn_entity = Node(
|
||||||
|
package='ros_gz_sim',
|
||||||
|
executable='create',
|
||||||
|
arguments=[
|
||||||
|
'-name', 'iisy',
|
||||||
|
'-allow_renaming', 'true',
|
||||||
|
'-string', xacro.process(os.path.join(get_package_share_directory('iisy_config_2'), 'config', 'iisy.urdf.xacro')),
|
||||||
|
],
|
||||||
|
output='screen'
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
|
||||||
|
launch_arguments={'gz_args': '-v 4 -r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='ros_gz_bridge',
|
||||||
|
executable='parameter_bridge',
|
||||||
|
arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
|
||||||
|
output='both',
|
||||||
|
),
|
||||||
|
|
||||||
|
SetParameter(name="use_sim_time", value=True),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='ros_actor_action_server',
|
||||||
|
executable='ros_actor_action_server'
|
||||||
|
),
|
||||||
|
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
str(moveit_config.package_path / "launch/rsp.launch.py")
|
||||||
|
),
|
||||||
|
),
|
||||||
|
|
||||||
|
#IncludeLaunchDescription(
|
||||||
|
# PythonLaunchDescriptionSource(
|
||||||
|
# str(moveit_config.package_path / "launch/move_group.launch.py")
|
||||||
|
# ),
|
||||||
|
#),
|
||||||
|
|
||||||
|
RegisterEventHandler(
|
||||||
|
event_handler=OnProcessExit(
|
||||||
|
target_action=ignition_spawn_entity,
|
||||||
|
on_exit=[load_joint_state_broadcaster],
|
||||||
|
)
|
||||||
|
),
|
||||||
|
RegisterEventHandler(
|
||||||
|
event_handler=OnProcessExit(
|
||||||
|
target_action=load_joint_state_broadcaster,
|
||||||
|
on_exit=[load_joint_trajectory_controller],
|
||||||
|
)
|
||||||
|
),
|
||||||
|
|
||||||
|
ignition_spawn_entity,
|
||||||
|
])
|
||||||
@ -1,15 +1,17 @@
|
|||||||
import os
|
import os
|
||||||
import subprocess
|
import subprocess
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch_ros.actions import Node,SetParameter
|
from launch_ros.actions import Node, SetParameter
|
||||||
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable
|
from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import PathJoinSubstitution
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
from launch.event_handlers import OnProcessExit
|
from launch.event_handlers import OnProcessExit
|
||||||
|
from launch.launch_description_entity import LaunchDescriptionEntity
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
def load_file(package_name, file_path):
|
def load_file(package_name, file_path):
|
||||||
@ -49,6 +51,19 @@ def load_yaml(package_name, file_path):
|
|||||||
return None
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def launch_chain(nodes: list[ExecuteLocal]):
|
||||||
|
generatedNodes: list[LaunchDescriptionEntity] = []
|
||||||
|
for index in range(1, len(nodes)):
|
||||||
|
generatedNodes.append(RegisterEventHandler(
|
||||||
|
OnProcessExit(
|
||||||
|
target_action=nodes[index-1],
|
||||||
|
on_exit=nodes[index]
|
||||||
|
)
|
||||||
|
))
|
||||||
|
generatedNodes.append(nodes[0])
|
||||||
|
return generatedNodes
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
|
||||||
robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
|
robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
|
||||||
@ -60,10 +75,9 @@ def generate_launch_description():
|
|||||||
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
|
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
|
||||||
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
|
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
|
||||||
|
|
||||||
moveit_controllers_yaml = load_yaml("iisy_config", "config/iisy_moveit_controllers.yaml")
|
controllers = {
|
||||||
moveit_controllers = {
|
"moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"),
|
||||||
"moveit_simple_controller_manager": moveit_controllers_yaml,
|
"moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager",
|
||||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
|
|
||||||
}
|
}
|
||||||
|
|
||||||
ompl_planning_pipeline_config = {
|
ompl_planning_pipeline_config = {
|
||||||
@ -86,7 +100,7 @@ def generate_launch_description():
|
|||||||
}
|
}
|
||||||
|
|
||||||
trajectory_execution = {
|
trajectory_execution = {
|
||||||
'moveit_manage_controllers': True,
|
'moveit_manage_controllers': False,
|
||||||
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
|
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
|
||||||
'trajectory_execution.allowed_goal_duration_margin': 0.5,
|
'trajectory_execution.allowed_goal_duration_margin': 0.5,
|
||||||
'trajectory_execution.allowed_start_tolerance': 0.01
|
'trajectory_execution.allowed_start_tolerance': 0.01
|
||||||
@ -126,11 +140,71 @@ def generate_launch_description():
|
|||||||
robot_description_kinematics,
|
robot_description_kinematics,
|
||||||
planning,
|
planning,
|
||||||
trajectory_execution,
|
trajectory_execution,
|
||||||
moveit_controllers,
|
controllers,
|
||||||
planning_scene_monitor_parameters,
|
planning_scene_monitor_parameters,
|
||||||
],
|
],
|
||||||
)
|
)
|
||||||
|
|
||||||
|
controller_manager = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="ros2_control_node",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
load_joint_state_broadcaster = ExecuteProcess(
|
||||||
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
|
||||||
|
'joint_state_broadcaster'],
|
||||||
|
output='both'
|
||||||
|
)
|
||||||
|
|
||||||
|
load_joint_trajectory_controller = ExecuteProcess(
|
||||||
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
|
||||||
|
'joint_trajectory_controller'],
|
||||||
|
output='both'
|
||||||
|
)
|
||||||
|
|
||||||
|
move_group = Node(
|
||||||
|
package='moveit_ros_move_group',
|
||||||
|
executable='move_group',
|
||||||
|
# prefix='xterm -fs 10 -e gdb --ex run --args',
|
||||||
|
output='both',
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_planning,
|
||||||
|
ompl_planning_pipeline_config,
|
||||||
|
robot_description_kinematics,
|
||||||
|
planning,
|
||||||
|
trajectory_execution,
|
||||||
|
controllers,
|
||||||
|
planning_scene_monitor_parameters,
|
||||||
|
# octomap_config,
|
||||||
|
# octomap_updater_config
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
test = Node(
|
||||||
|
package='moveit_test',
|
||||||
|
executable='move',
|
||||||
|
output='both',
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_planning,
|
||||||
|
ompl_planning_pipeline_config,
|
||||||
|
robot_description_kinematics,
|
||||||
|
planning,
|
||||||
|
trajectory_execution,
|
||||||
|
controllers,
|
||||||
|
planning_scene_monitor_parameters,
|
||||||
|
# octomap_config,
|
||||||
|
# octomap_updater_config
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
# octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
|
# octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
|
||||||
# 'octomap_resolution': 0.05,
|
# 'octomap_resolution': 0.05,
|
||||||
# 'max_range': 5.0}
|
# 'max_range': 5.0}
|
||||||
@ -139,19 +213,55 @@ def generate_launch_description():
|
|||||||
|
|
||||||
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
||||||
|
|
||||||
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
SetParameter(name="use_sim_time",value=True),
|
|
||||||
|
#IncludeLaunchDescription(
|
||||||
|
# PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
|
||||||
|
# launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
|
||||||
|
#),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='ros_gz_bridge',
|
||||||
|
executable='parameter_bridge',
|
||||||
|
arguments=["/clock[rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
|
||||||
|
output='both',
|
||||||
|
),
|
||||||
|
|
||||||
|
SetParameter(name="use_sim_time", value=True),
|
||||||
|
|
||||||
|
move_group,
|
||||||
|
|
||||||
|
#RegisterEventHandler(
|
||||||
|
# OnProcessExit(
|
||||||
|
# target_action=spawn_robot,
|
||||||
|
# on_exit=[load_joint_state_broadcaster]
|
||||||
|
# )
|
||||||
|
#),
|
||||||
|
|
||||||
|
controller_manager,
|
||||||
|
|
||||||
|
RegisterEventHandler(
|
||||||
|
OnProcessExit(
|
||||||
|
target_action=load_joint_state_broadcaster,
|
||||||
|
on_exit=[load_joint_trajectory_controller]
|
||||||
|
)
|
||||||
|
),
|
||||||
|
RegisterEventHandler(
|
||||||
|
OnProcessExit(
|
||||||
|
target_action=load_joint_trajectory_controller,
|
||||||
|
on_exit=TimerAction(period=10.0,actions=[test])
|
||||||
|
)
|
||||||
|
),
|
||||||
|
|
||||||
|
#spawn_robot,
|
||||||
|
load_joint_state_broadcaster,
|
||||||
|
#load_joint_trajectory_controller,
|
||||||
|
rviz,
|
||||||
|
|
||||||
Node(
|
Node(
|
||||||
package='ros_actor_action_server',
|
package='ros_actor_action_server',
|
||||||
executable='ros_actor_action_server'),
|
executable='ros_actor_action_server'),
|
||||||
|
|
||||||
IncludeLaunchDescription(
|
|
||||||
PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
|
|
||||||
launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
|
|
||||||
),
|
|
||||||
|
|
||||||
Node(
|
Node(
|
||||||
package='tf2_ros',
|
package='tf2_ros',
|
||||||
executable='static_transform_publisher',
|
executable='static_transform_publisher',
|
||||||
@ -182,20 +292,15 @@ def generate_launch_description():
|
|||||||
"--child-frame-id", "lidar_2_link"]
|
"--child-frame-id", "lidar_2_link"]
|
||||||
),
|
),
|
||||||
|
|
||||||
Node(
|
#Node(
|
||||||
package='ros_gz_bridge',
|
# package='ros_gz_bridge',
|
||||||
executable='parameter_bridge',
|
# executable='parameter_bridge',
|
||||||
arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
|
# arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"],
|
||||||
),
|
# output='both',
|
||||||
|
# remappings=[
|
||||||
Node(
|
# ('/world/default/model/iisy/joint_state', '/joint_states'),
|
||||||
package='ros_gz_bridge',
|
# ]
|
||||||
executable='parameter_bridge',
|
#),
|
||||||
arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"],
|
|
||||||
remappings=[
|
|
||||||
('/world/default/model/iisy/joint_state', '/joint_states'),
|
|
||||||
]
|
|
||||||
),
|
|
||||||
|
|
||||||
Node(
|
Node(
|
||||||
package='robot_state_publisher',
|
package='robot_state_publisher',
|
||||||
@ -203,28 +308,4 @@ def generate_launch_description():
|
|||||||
output='both',
|
output='both',
|
||||||
parameters=[robot_description]
|
parameters=[robot_description]
|
||||||
),
|
),
|
||||||
|
|
||||||
Node(
|
|
||||||
package='moveit_ros_move_group',
|
|
||||||
executable='move_group',
|
|
||||||
# prefix='xterm -fs 10 -e gdb --ex run --args',
|
|
||||||
output='both',
|
|
||||||
parameters=[
|
|
||||||
robot_description,
|
|
||||||
robot_description_semantic,
|
|
||||||
robot_description_planning,
|
|
||||||
ompl_planning_pipeline_config,
|
|
||||||
robot_description_kinematics,
|
|
||||||
planning,
|
|
||||||
trajectory_execution,
|
|
||||||
moveit_controllers,
|
|
||||||
planning_scene_monitor_parameters,
|
|
||||||
# octomap_config,
|
|
||||||
# octomap_updater_config
|
|
||||||
]
|
|
||||||
),
|
|
||||||
spawn_robot,
|
|
||||||
rviz,
|
|
||||||
|
|
||||||
|
|
||||||
])
|
])
|
||||||
|
|||||||
55
src/ign_world/launch/new_launch.py
Normal file
55
src/ign_world/launch/new_launch.py
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
import os
|
||||||
|
from ament_index_python import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import IncludeLaunchDescription
|
||||||
|
from launch_ros.actions import Node, SetParameter
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from xacro import process as xacro_process
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
SetParameter(name="use_sim_time", value=True),
|
||||||
|
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
|
||||||
|
launch_arguments={'gz_args': '-v 4 -r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='ros_gz_bridge',
|
||||||
|
executable='parameter_bridge',
|
||||||
|
arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
|
||||||
|
output='both',
|
||||||
|
),
|
||||||
|
|
||||||
|
#Node(
|
||||||
|
# package='ros_gz_bridge',
|
||||||
|
# executable='parameter_bridge',
|
||||||
|
# arguments=["/world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage]ignition.msgs.Pose_V"],
|
||||||
|
# output='both',
|
||||||
|
# remappings=[
|
||||||
|
# ('/world/default/dynamic_pose/info', '/tf'),
|
||||||
|
# ]
|
||||||
|
#),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='ros_actor_action_server',
|
||||||
|
executable='ros_actor_action_server'
|
||||||
|
),
|
||||||
|
|
||||||
|
#Node(
|
||||||
|
# package='ros_gz_sim',
|
||||||
|
# executable='create',
|
||||||
|
# arguments=['-world', 'default', '-string', xacro_process(os.path.join(get_package_share_directory('iisy_config_2'), 'config', 'iisy_xacro.urdf'))],
|
||||||
|
# output='screen'
|
||||||
|
#),
|
||||||
|
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
str(moveit_config.package_path / "launch/demo.launch.py")
|
||||||
|
),
|
||||||
|
),
|
||||||
|
])
|
||||||
@ -1,19 +1,16 @@
|
|||||||
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
|
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
|
||||||
controller_manager:
|
controller_manager:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
update_rate: 500 # Hz
|
update_rate: 1000 # Hz
|
||||||
|
|
||||||
joint_state_broadcaster:
|
joint_state_broadcaster:
|
||||||
type: joint_state_broadcaster/JointStateBroadcaster # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41
|
type: joint_state_broadcaster/JointStateBroadcaster # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41
|
||||||
|
|
||||||
position_trajectory_controller:
|
joint_trajectory_controller:
|
||||||
# find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2
|
# find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2
|
||||||
type: joint_trajectory_controller/JointTrajectoryController
|
type: joint_trajectory_controller/JointTrajectoryController
|
||||||
|
|
||||||
forward_position_controller:
|
joint_trajectory_controller:
|
||||||
type: position_controllers/JointGroupPositionController
|
|
||||||
|
|
||||||
position_trajectory_controller:
|
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
# find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp
|
# find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp
|
||||||
joints:
|
joints:
|
||||||
@ -27,16 +24,4 @@ position_trajectory_controller:
|
|||||||
- position
|
- position
|
||||||
state_interfaces:
|
state_interfaces:
|
||||||
- position
|
- position
|
||||||
state_publish_rate: 50.0
|
- velocity
|
||||||
action_monitor_rate: 20.0
|
|
||||||
|
|
||||||
forward_position_controller:
|
|
||||||
ros__parameters:
|
|
||||||
joints:
|
|
||||||
- base_rot
|
|
||||||
- base_link1_joint
|
|
||||||
- link1_link2_joint
|
|
||||||
- link2_rot
|
|
||||||
- link2_link3_joint
|
|
||||||
- link3_rot
|
|
||||||
interface_name: "position"
|
|
||||||
|
|||||||
@ -1,7 +1,7 @@
|
|||||||
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
|
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
|
||||||
controller_manager:
|
controller_manager:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
update_rate: 500 # Hz
|
update_rate: 1000 # Hz
|
||||||
|
|
||||||
joint_state_controller:
|
joint_state_controller:
|
||||||
type: joint_state_controller/JointStateController # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41
|
type: joint_state_controller/JointStateController # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41
|
||||||
@ -1,9 +1,8 @@
|
|||||||
controller_names:
|
controller_names:
|
||||||
- position_trajectory_controller
|
- joint_trajectory_controller
|
||||||
|
|
||||||
# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller
|
# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller
|
||||||
position_trajectory_controller:
|
joint_trajectory_controller:
|
||||||
action_ns: follow_joint_trajectory
|
|
||||||
type: FollowJointTrajectory
|
type: FollowJointTrajectory
|
||||||
default: true
|
default: true
|
||||||
joints:
|
joints:
|
||||||
|
|||||||
@ -290,97 +290,50 @@
|
|||||||
</sensor>
|
</sensor>
|
||||||
<!-- define joints and command/state interfaces for each joint -->
|
<!-- define joints and command/state interfaces for each joint -->
|
||||||
<joint name="base_rot">
|
<joint name="base_rot">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<!-- better to use radians as min max first -->
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
<joint name="base_link1_joint">
|
<joint name="base_link1_joint">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
<joint name="link1_link2_joint">
|
<joint name="link1_link2_joint">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
<joint name="link2_rot">
|
<joint name="link2_rot">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
<joint name="link2_link3_joint">
|
<joint name="link2_link3_joint">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
<joint name="link3_rot">
|
<joint name="link3_rot">
|
||||||
<command_interface name="position">
|
<command_interface name="position"/>
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<command_interface name="effort">
|
|
||||||
<param name="min">-1</param>
|
|
||||||
<param name="max"> 1</param>
|
|
||||||
</command_interface>
|
|
||||||
<state_interface name="position"/>
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
<state_interface name="external_torque"/>
|
|
||||||
</joint>
|
</joint>
|
||||||
</ros2_control>
|
</ros2_control>
|
||||||
|
|
||||||
<gazebo>
|
<gazebo>
|
||||||
<plugin filename="ignition-gazebo-joint-state-publisher-system" name="ignition::gazebo::systems::JointStatePublisher"/>
|
<plugin filename="$(find ign_ros2_control)/../../lib/libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo>
|
|
||||||
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
|
||||||
<parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
|
<parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
|
||||||
</plugin>
|
</plugin>
|
||||||
</gazebo>
|
</gazebo>
|
||||||
|
|
||||||
<gazebo reference="base_bottom">
|
<gazebo reference="base_bottom">lsl
|
||||||
<mu1>0.2</mu1>
|
<mu1>0.2</mu1>
|
||||||
<mu2>0.2</mu2>
|
<mu2>0.2</mu2>
|
||||||
<material>Gazebo/Orange</material>
|
<material>Gazebo/Orange</material>
|
||||||
|
|||||||
397
src/iisy_config/urdf/iisy_xacro.urdf
Normal file
397
src/iisy_config/urdf/iisy_xacro.urdf
Normal file
@ -0,0 +1,397 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from src/iisy_config/urdf/iisy.urdf | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="iisy">
|
||||||
|
<link name="world"/>
|
||||||
|
<link name="base_link"/>
|
||||||
|
<link name="table">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="base_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
|
||||||
|
<mass value="6"/>
|
||||||
|
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="base_top">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 -0.005 0.08"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.15 0.16"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link1_full">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
|
||||||
|
<mass value="4"/>
|
||||||
|
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_bottom">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0.07 0.025"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.14 0.19"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.07 0.025"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
|
||||||
|
<mass value="0.8"/>
|
||||||
|
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="world_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="table"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="table_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 1"/>
|
||||||
|
<parent link="table"/>
|
||||||
|
<child link="base_bottom"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.1205"/>
|
||||||
|
<parent link="base_bottom"/>
|
||||||
|
<child link="base_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
|
||||||
|
<parent link="base_top"/>
|
||||||
|
<child link="link1_full"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.3"/>
|
||||||
|
<parent link="link1_full"/>
|
||||||
|
<child link="link2_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
|
||||||
|
<parent link="link2_bottom"/>
|
||||||
|
<child link="link2_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.0565 0.178003"/>
|
||||||
|
<parent link="link2_top"/>
|
||||||
|
<child link="link3_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
|
||||||
|
<parent link="link3_bottom"/>
|
||||||
|
<child link="link3_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<transmission name="trans_base_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_base_link1_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_link1_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link1_link2_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link1_link2_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_link3_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_link3_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link3_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link3_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<ros2_control name="IgnitionSystem" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>ign_ros2_control/IgnitionSystem</plugin>
|
||||||
|
</hardware>
|
||||||
|
<!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
|
||||||
|
<sensor name="iisy_state">
|
||||||
|
<!-- see KUKA::FRI::LBRState -->
|
||||||
|
<state_interface name="sample_time"/>
|
||||||
|
<state_interface name="time_stamp_sec"/>
|
||||||
|
<state_interface name="time_stamp_nano_sec"/>
|
||||||
|
</sensor>
|
||||||
|
<!-- define joints and command/state interfaces for each joint -->
|
||||||
|
<joint name="base_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<!-- better to use radians as min max first -->
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="effort">
|
||||||
|
<param name="min">-1</param>
|
||||||
|
<param name="max"> 1</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="effort"/>
|
||||||
|
<state_interface name="external_torque"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
||||||
|
<parameters>/home/ros/workspace/install/iisy_config/share/iisy_config/config/iisy_controllers.yml</parameters>
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link1_full">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Grey</material>
|
||||||
|
</gazebo>
|
||||||
|
</robot>
|
||||||
25
src/iisy_config_2/.setup_assistant
Normal file
25
src/iisy_config_2/.setup_assistant
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
moveit_setup_assistant_config:
|
||||||
|
urdf:
|
||||||
|
package: iisy_config
|
||||||
|
relative_path: urdf/iisy_xacro.urdf
|
||||||
|
srdf:
|
||||||
|
relative_path: config/iisy.srdf
|
||||||
|
package_settings:
|
||||||
|
author_name: Bastian Hofmann
|
||||||
|
author_email: bhogm4@gmail.com
|
||||||
|
generated_timestamp: 1676632718
|
||||||
|
control_xacro:
|
||||||
|
command:
|
||||||
|
- position
|
||||||
|
state:
|
||||||
|
- position
|
||||||
|
- velocity
|
||||||
|
modified_urdf:
|
||||||
|
xacros:
|
||||||
|
- control_xacro
|
||||||
|
control_xacro:
|
||||||
|
command:
|
||||||
|
- position
|
||||||
|
state:
|
||||||
|
- position
|
||||||
|
- velocity
|
||||||
11
src/iisy_config_2/CMakeLists.txt
Normal file
11
src/iisy_config_2/CMakeLists.txt
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.22)
|
||||||
|
project(iisy_config_2)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
|
|
||||||
|
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
|
||||||
|
PATTERN "setup_assistant.launch" EXCLUDE)
|
||||||
|
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
||||||
|
install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
|
||||||
62
src/iisy_config_2/config/iisy.ros2_control.xacro
Normal file
62
src/iisy_config_2/config/iisy.ros2_control.xacro
Normal file
@ -0,0 +1,62 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
<xacro:macro name="iisy_ros2_control" params="name initial_positions_file">
|
||||||
|
<xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
|
||||||
|
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
||||||
|
<parameters>$(find iisy_config_2)/config/ros2_controllers.yaml</parameters>
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
<ros2_control name="IgnitionSystem" type="system">
|
||||||
|
<hardware>
|
||||||
|
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
|
||||||
|
<plugin>ign_ros2_control/IgnitionSystem</plugin>
|
||||||
|
</hardware>
|
||||||
|
<joint name="base_rot">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['base_rot']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['base_link1_joint']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['link1_link2_joint']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['link2_rot']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['link2_link3_joint']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">${initial_positions['link3_rot']}</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</ros2_control>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
34
src/iisy_config_2/config/iisy.srdf
Normal file
34
src/iisy_config_2/config/iisy.srdf
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<!--This does not replace URDF, and is not an extension of URDF.
|
||||||
|
This is a format for representing semantic information about the robot structure.
|
||||||
|
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||||
|
-->
|
||||||
|
<robot name="iisy">
|
||||||
|
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||||
|
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||||
|
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||||
|
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||||
|
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||||
|
<group name="arm">
|
||||||
|
<joint name="base_rot"/>
|
||||||
|
<joint name="base_link1_joint"/>
|
||||||
|
<joint name="link1_link2_joint"/>
|
||||||
|
<joint name="link2_rot"/>
|
||||||
|
<joint name="link2_link3_joint"/>
|
||||||
|
<joint name="link3_rot"/>
|
||||||
|
<chain base_link="base_bottom" tip_link="link3_top"/>
|
||||||
|
</group>
|
||||||
|
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||||
|
<end_effector name="tip" parent_link="link3_top" group="arm"/>
|
||||||
|
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||||
|
<disable_collisions link1="base_bottom" link2="base_top" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="base_bottom" link2="table" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="base_top" link2="link1_full" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="base_top" link2="link2_bottom" reason="Never"/>
|
||||||
|
<disable_collisions link1="base_top" link2="table" reason="Never"/>
|
||||||
|
<disable_collisions link1="link1_full" link2="link2_bottom" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="link2_bottom" link2="link2_top" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="link2_bottom" link2="link3_bottom" reason="Never"/>
|
||||||
|
<disable_collisions link1="link2_top" link2="link3_bottom" reason="Adjacent"/>
|
||||||
|
<disable_collisions link1="link3_bottom" link2="link3_top" reason="Adjacent"/>
|
||||||
|
</robot>
|
||||||
14
src/iisy_config_2/config/iisy.urdf.xacro
Normal file
14
src/iisy_config_2/config/iisy.urdf.xacro
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iisy">
|
||||||
|
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
|
||||||
|
|
||||||
|
<!-- Import iisy urdf file -->
|
||||||
|
<xacro:include filename="$(find iisy_config_2)/config/iisy_xacro.urdf" />
|
||||||
|
|
||||||
|
<!-- Import control_xacro -->
|
||||||
|
<xacro:include filename="iisy.ros2_control.xacro" />
|
||||||
|
|
||||||
|
|
||||||
|
<xacro:iisy_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
|
||||||
|
|
||||||
|
</robot>
|
||||||
300
src/iisy_config_2/config/iisy_xacro.urdf
Normal file
300
src/iisy_config_2/config/iisy_xacro.urdf
Normal file
@ -0,0 +1,300 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from src/iisy_config/urdf/iisy.urdf | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="iisy">
|
||||||
|
<link name="world"/>
|
||||||
|
<link name="base_link"/>
|
||||||
|
<link name="table">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/table.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="base_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
|
||||||
|
<mass value="6"/>
|
||||||
|
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="base_top">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 -0.005 0.08"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.15 0.16"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/base_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link1_full">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_1_full_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
|
||||||
|
<mass value="4"/>
|
||||||
|
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_bottom">
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0.07 0.025"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.14 0.14 0.19"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.07 0.025"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link2_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_2_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_bottom">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_bottom_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<link name="link3_top">
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_coll.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:///home/ros/workspace/install/iisy_config/share/iisy_config/stl/link_3_top_low.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
|
||||||
|
<mass value="0.8"/>
|
||||||
|
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="world_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_base_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="table"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="table_link_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 1"/>
|
||||||
|
<parent link="table"/>
|
||||||
|
<child link="base_bottom"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.1205"/>
|
||||||
|
<parent link="base_bottom"/>
|
||||||
|
<child link="base_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="base_link1_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
|
||||||
|
<parent link="base_top"/>
|
||||||
|
<child link="link1_full"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link1_link2_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.3"/>
|
||||||
|
<parent link="link1_full"/>
|
||||||
|
<child link="link2_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
|
||||||
|
<parent link="link2_bottom"/>
|
||||||
|
<child link="link2_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link2_link3_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.0565 0.178003"/>
|
||||||
|
<parent link="link2_top"/>
|
||||||
|
<child link="link3_bottom"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="link3_rot" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
|
||||||
|
<parent link="link3_bottom"/>
|
||||||
|
<child link="link3_top"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="30" velocity="1.7453292519943"/>
|
||||||
|
<dynamics damping="10.0" friction="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<transmission name="trans_base_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_base_link1_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="base_link1_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="base_link1_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link1_link2_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link1_link2_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link1_link2_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link2_link3_joint">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link2_link3_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link2_link3_joint_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="trans_link3_rot">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="link3_rot">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="link3_rot_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<gazebo reference="base_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="base_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link1_full">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_bottom">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Orange</material>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3_top">
|
||||||
|
<mu1>0.2</mu1>
|
||||||
|
<mu2>0.2</mu2>
|
||||||
|
<material>Gazebo/Grey</material>
|
||||||
|
</gazebo>
|
||||||
|
</robot>
|
||||||
9
src/iisy_config_2/config/initial_positions.yaml
Normal file
9
src/iisy_config_2/config/initial_positions.yaml
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
# Default initial positions for iisy's ros2_control fake system
|
||||||
|
|
||||||
|
initial_positions:
|
||||||
|
base_link1_joint: 0
|
||||||
|
base_rot: 0
|
||||||
|
link1_link2_joint: 0
|
||||||
|
link2_link3_joint: 0
|
||||||
|
link2_rot: 0
|
||||||
|
link3_rot: 0
|
||||||
40
src/iisy_config_2/config/joint_limits.yaml
Normal file
40
src/iisy_config_2/config/joint_limits.yaml
Normal file
@ -0,0 +1,40 @@
|
|||||||
|
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
||||||
|
|
||||||
|
# For beginners, we downscale velocity and acceleration limits.
|
||||||
|
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
||||||
|
default_velocity_scaling_factor: 0.1
|
||||||
|
default_acceleration_scaling_factor: 0.1
|
||||||
|
|
||||||
|
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
||||||
|
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||||
|
joint_limits:
|
||||||
|
base_link1_joint:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
|
base_rot:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
|
link1_link2_joint:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
|
link2_link3_joint:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
|
link2_rot:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
|
link3_rot:
|
||||||
|
has_velocity_limits: true
|
||||||
|
max_velocity: 1.7453292519943
|
||||||
|
has_acceleration_limits: false
|
||||||
|
max_acceleration: 0
|
||||||
1
src/iisy_config_2/config/kinematics.yaml
Normal file
1
src/iisy_config_2/config/kinematics.yaml
Normal file
@ -0,0 +1 @@
|
|||||||
|
{}
|
||||||
281
src/iisy_config_2/config/moveit.rviz
Normal file
281
src/iisy_config_2/config/moveit.rviz
Normal file
@ -0,0 +1,281 @@
|
|||||||
|
Panels:
|
||||||
|
- Class: rviz_common/Displays
|
||||||
|
Help Height: 70
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /MotionPlanning1
|
||||||
|
- /InteractiveMarkers1
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Tree Height: 163
|
||||||
|
- Class: rviz_common/Help
|
||||||
|
Name: Help
|
||||||
|
- Class: rviz_common/Views
|
||||||
|
Expanded:
|
||||||
|
- /Current View1
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- Alpha: 0.5
|
||||||
|
Cell Size: 1
|
||||||
|
Class: rviz_default_plugins/Grid
|
||||||
|
Color: 160; 160; 164
|
||||||
|
Enabled: true
|
||||||
|
Line Style:
|
||||||
|
Line Width: 0.029999999329447746
|
||||||
|
Value: Lines
|
||||||
|
Name: Grid
|
||||||
|
Normal Cell Count: 0
|
||||||
|
Offset:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Plane: XY
|
||||||
|
Plane Cell Count: 10
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Acceleration_Scaling_Factor: 0.1
|
||||||
|
Class: moveit_rviz_plugin/MotionPlanning
|
||||||
|
Enabled: true
|
||||||
|
Move Group Namespace: ""
|
||||||
|
MoveIt_Allow_Approximate_IK: false
|
||||||
|
MoveIt_Allow_External_Program: false
|
||||||
|
MoveIt_Allow_Replanning: false
|
||||||
|
MoveIt_Allow_Sensor_Positioning: false
|
||||||
|
MoveIt_Planning_Attempts: 10
|
||||||
|
MoveIt_Planning_Time: 5
|
||||||
|
MoveIt_Use_Cartesian_Path: false
|
||||||
|
MoveIt_Use_Constraint_Aware_IK: false
|
||||||
|
MoveIt_Workspace:
|
||||||
|
Center:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Size:
|
||||||
|
X: 2
|
||||||
|
Y: 2
|
||||||
|
Z: 2
|
||||||
|
Name: MotionPlanning
|
||||||
|
Planned Path:
|
||||||
|
Color Enabled: false
|
||||||
|
Interrupt Display: false
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
base_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link1_full:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link2_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link2_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link3_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link3_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
table:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Loop Animation: true
|
||||||
|
Robot Alpha: 0.5
|
||||||
|
Robot Color: 150; 50; 150
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Show Trail: false
|
||||||
|
State Display Time: 0.05 s
|
||||||
|
Trail Step Size: 1
|
||||||
|
Trajectory Topic: display_planned_path
|
||||||
|
Use Sim Time: false
|
||||||
|
Planning Metrics:
|
||||||
|
Payload: 1
|
||||||
|
Show Joint Torques: false
|
||||||
|
Show Manipulability: false
|
||||||
|
Show Manipulability Index: false
|
||||||
|
Show Weight Limit: false
|
||||||
|
TextHeight: 0.07999999821186066
|
||||||
|
Planning Request:
|
||||||
|
Colliding Link Color: 255; 0; 0
|
||||||
|
Goal State Alpha: 1
|
||||||
|
Goal State Color: 250; 128; 0
|
||||||
|
Interactive Marker Size: 0
|
||||||
|
Joint Violation Color: 255; 0; 255
|
||||||
|
Planning Group: arm
|
||||||
|
Query Goal State: true
|
||||||
|
Query Start State: false
|
||||||
|
Show Workspace: false
|
||||||
|
Start State Alpha: 1
|
||||||
|
Start State Color: 0; 255; 0
|
||||||
|
Planning Scene Topic: monitored_planning_scene
|
||||||
|
Robot Description: robot_description
|
||||||
|
Scene Geometry:
|
||||||
|
Scene Alpha: 1
|
||||||
|
Scene Color: 50; 230; 50
|
||||||
|
Scene Display Time: 0.009999999776482582
|
||||||
|
Show Scene Geometry: true
|
||||||
|
Voxel Coloring: Z-Axis
|
||||||
|
Voxel Rendering: Occupied Voxels
|
||||||
|
Scene Robot:
|
||||||
|
Attached Body Color: 150; 50; 150
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
base_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link1_full:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link2_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link2_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link3_bottom:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
link3_top:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
table:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Robot Alpha: 0.5
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Value: true
|
||||||
|
Velocity_Scaling_Factor: 0.9999999999999999
|
||||||
|
- Class: rviz_default_plugins/InteractiveMarkers
|
||||||
|
Enable Transparency: true
|
||||||
|
Enabled: true
|
||||||
|
Interactive Markers Namespace: ""
|
||||||
|
Name: InteractiveMarkers
|
||||||
|
Show Axes: false
|
||||||
|
Show Descriptions: true
|
||||||
|
Show Visual Aids: false
|
||||||
|
Value: true
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 48; 48; 48
|
||||||
|
Fixed Frame: world
|
||||||
|
Frame Rate: 30
|
||||||
|
Name: root
|
||||||
|
Tools:
|
||||||
|
- Class: rviz_default_plugins/Interact
|
||||||
|
Hide Inactive Objects: true
|
||||||
|
- Class: rviz_default_plugins/MoveCamera
|
||||||
|
- Class: rviz_default_plugins/Select
|
||||||
|
Transformation:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/TF
|
||||||
|
Value: true
|
||||||
|
Views:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/Orbit
|
||||||
|
Distance: 4.384417533874512
|
||||||
|
Enable Stereo Rendering:
|
||||||
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
|
Stereo Focal Distance: 1
|
||||||
|
Swap Stereo Eyes: false
|
||||||
|
Value: false
|
||||||
|
Focal Point:
|
||||||
|
X: -0.10000000149011612
|
||||||
|
Y: 0.25
|
||||||
|
Z: 0.30000001192092896
|
||||||
|
Focal Shape Fixed Size: true
|
||||||
|
Focal Shape Size: 0.05000000074505806
|
||||||
|
Invert Z Axis: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.009999999776482582
|
||||||
|
Pitch: 0.4799998998641968
|
||||||
|
Target Frame: world
|
||||||
|
Value: Orbit (rviz_default_plugins)
|
||||||
|
Yaw: 3.7451796531677246
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: false
|
||||||
|
Height: 975
|
||||||
|
Help:
|
||||||
|
collapsed: false
|
||||||
|
Hide Left Dock: false
|
||||||
|
Hide Right Dock: false
|
||||||
|
MotionPlanning:
|
||||||
|
collapsed: false
|
||||||
|
MotionPlanning - Trajectory Slider:
|
||||||
|
collapsed: false
|
||||||
|
QMainWindow State: 000000ff00000000fd0000000100000000000002b400000379fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000100044006900730070006c006100790073010000003b00000124000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000165000001930000016a00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fe000000b6000000a000ffffff000001f60000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Views:
|
||||||
|
collapsed: false
|
||||||
|
Width: 1200
|
||||||
|
X: 1360
|
||||||
|
Y: 388
|
||||||
19
src/iisy_config_2/config/moveit_controllers.yaml
Normal file
19
src/iisy_config_2/config/moveit_controllers.yaml
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
# MoveIt uses this configuration for controller management
|
||||||
|
|
||||||
|
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
|
||||||
|
|
||||||
|
moveit_simple_controller_manager:
|
||||||
|
controller_names:
|
||||||
|
- arm_controller
|
||||||
|
|
||||||
|
arm_controller:
|
||||||
|
type: FollowJointTrajectory
|
||||||
|
action_ns: follow_joint_trajectory
|
||||||
|
default: true
|
||||||
|
joints:
|
||||||
|
- base_rot
|
||||||
|
- base_link1_joint
|
||||||
|
- link1_link2_joint
|
||||||
|
- link2_rot
|
||||||
|
- link2_link3_joint
|
||||||
|
- link3_rot
|
||||||
6
src/iisy_config_2/config/pilz_cartesian_limits.yaml
Normal file
6
src/iisy_config_2/config/pilz_cartesian_limits.yaml
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
# Limits for the Pilz planner
|
||||||
|
cartesian_limits:
|
||||||
|
max_trans_vel: 1.0
|
||||||
|
max_trans_acc: 2.25
|
||||||
|
max_trans_dec: -5.0
|
||||||
|
max_rot_vel: 1.57
|
||||||
26
src/iisy_config_2/config/ros2_controllers.yaml
Normal file
26
src/iisy_config_2/config/ros2_controllers.yaml
Normal file
@ -0,0 +1,26 @@
|
|||||||
|
# This config file is used by ros2_control
|
||||||
|
controller_manager:
|
||||||
|
ros__parameters:
|
||||||
|
update_rate: 100 # Hz
|
||||||
|
|
||||||
|
arm_controller:
|
||||||
|
type: joint_trajectory_controller/JointTrajectoryController
|
||||||
|
|
||||||
|
|
||||||
|
joint_state_broadcaster:
|
||||||
|
type: joint_state_broadcaster/JointStateBroadcaster
|
||||||
|
|
||||||
|
arm_controller:
|
||||||
|
ros__parameters:
|
||||||
|
joints:
|
||||||
|
- base_rot
|
||||||
|
- base_link1_joint
|
||||||
|
- link1_link2_joint
|
||||||
|
- link2_rot
|
||||||
|
- link2_link3_joint
|
||||||
|
- link3_rot
|
||||||
|
command_interfaces:
|
||||||
|
- position
|
||||||
|
state_interfaces:
|
||||||
|
- position
|
||||||
|
- velocity
|
||||||
7
src/iisy_config_2/launch/demo.launch.py
Normal file
7
src/iisy_config_2/launch/demo.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_demo_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_demo_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/move_group.launch.py
Normal file
7
src/iisy_config_2/launch/move_group.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_move_group_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_move_group_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/moveit_rviz.launch.py
Normal file
7
src/iisy_config_2/launch/moveit_rviz.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_moveit_rviz_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_moveit_rviz_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/rsp.launch.py
Normal file
7
src/iisy_config_2/launch/rsp.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_rsp_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_rsp_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/setup_assistant.launch.py
Normal file
7
src/iisy_config_2/launch/setup_assistant.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_setup_assistant_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_setup_assistant_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/spawn_controllers.launch.py
Normal file
7
src/iisy_config_2/launch/spawn_controllers.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_spawn_controllers_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_spawn_controllers_launch(moveit_config)
|
||||||
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_static_virtual_joint_tfs_launch(moveit_config)
|
||||||
7
src/iisy_config_2/launch/warehouse_db.launch.py
Normal file
7
src/iisy_config_2/launch/warehouse_db.launch.py
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
from moveit_configs_utils import MoveItConfigsBuilder
|
||||||
|
from moveit_configs_utils.launches import generate_warehouse_db_launch
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
|
||||||
|
return generate_warehouse_db_launch(moveit_config)
|
||||||
52
src/iisy_config_2/package.xml
Normal file
52
src/iisy_config_2/package.xml
Normal file
@ -0,0 +1,52 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>iisy_config_2</name>
|
||||||
|
<version>0.3.0</version>
|
||||||
|
<description>
|
||||||
|
An automatically generated package with all the configuration and launch files for using the iisy with the MoveIt Motion Planning Framework
|
||||||
|
</description>
|
||||||
|
<maintainer email="bhogm4@gmail.com">Bastian Hofmann</maintainer>
|
||||||
|
|
||||||
|
<license>BSD</license>
|
||||||
|
|
||||||
|
<url type="website">http://moveit.ros.org/</url>
|
||||||
|
<url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
|
||||||
|
<url type="repository">https://github.com/ros-planning/moveit2</url>
|
||||||
|
|
||||||
|
<author email="bhogm4@gmail.com">Bastian Hofmann</author>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>moveit_ros_move_group</exec_depend>
|
||||||
|
<exec_depend>moveit_kinematics</exec_depend>
|
||||||
|
<exec_depend>moveit_planners</exec_depend>
|
||||||
|
<exec_depend>moveit_simple_controller_manager</exec_depend>
|
||||||
|
<exec_depend>joint_state_publisher</exec_depend>
|
||||||
|
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||||
|
<exec_depend>tf2_ros</exec_depend>
|
||||||
|
<exec_depend>xacro</exec_depend>
|
||||||
|
<!-- The next 2 packages are required for the gazebo simulation.
|
||||||
|
We don't include them by default to prevent installing gazebo and all its dependencies. -->
|
||||||
|
<!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
|
||||||
|
<!-- <exec_depend>gazebo_ros_control</exec_depend> -->
|
||||||
|
<exec_depend>controller_manager</exec_depend>
|
||||||
|
<exec_depend>iisy_config</exec_depend>
|
||||||
|
<exec_depend>moveit_configs_utils</exec_depend>
|
||||||
|
<exec_depend>moveit_ros_move_group</exec_depend>
|
||||||
|
<exec_depend>moveit_ros_visualization</exec_depend>
|
||||||
|
<exec_depend>moveit_ros_warehouse</exec_depend>
|
||||||
|
<exec_depend>moveit_setup_assistant</exec_depend>
|
||||||
|
<exec_depend>robot_state_publisher</exec_depend>
|
||||||
|
<exec_depend>rviz2</exec_depend>
|
||||||
|
<exec_depend>rviz_common</exec_depend>
|
||||||
|
<exec_depend>rviz_default_plugins</exec_depend>
|
||||||
|
<exec_depend>tf2_ros</exec_depend>
|
||||||
|
<exec_depend>warehouse_ros_mongo</exec_depend>
|
||||||
|
<exec_depend>xacro</exec_depend>
|
||||||
|
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
36
src/moveit_test/CMakeLists.txt
Normal file
36
src/moveit_test/CMakeLists.txt
Normal file
@ -0,0 +1,36 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(moveit_test)
|
||||||
|
|
||||||
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# find dependencies
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(moveit_ros_planning_interface REQUIRED)
|
||||||
|
find_package(geometry_msgs REQUIRED)
|
||||||
|
# uncomment the following section in order to fill in
|
||||||
|
# further dependencies manually.
|
||||||
|
# find_package(<dependency> REQUIRED)
|
||||||
|
|
||||||
|
add_executable(move src/test.cpp)
|
||||||
|
set_property(TARGET move PROPERTY CXX_STANDARD 17)
|
||||||
|
ament_target_dependencies(move rclcpp moveit_ros_planning_interface geometry_msgs)
|
||||||
|
|
||||||
|
install(TARGETS
|
||||||
|
move
|
||||||
|
DESTINATION lib/${PROJECT_NAME})
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_lint_auto REQUIRED)
|
||||||
|
# the following line skips the linter which checks for copyrights
|
||||||
|
# uncomment the line when a copyright and license is not present in all source files
|
||||||
|
#set(ament_cmake_copyright_FOUND TRUE)
|
||||||
|
# the following line skips cpplint (only works in a git repo)
|
||||||
|
# uncomment the line when this package is not in a git repo
|
||||||
|
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||||
|
ament_lint_auto_find_test_dependencies()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
ament_package()
|
||||||
23
src/moveit_test/package.xml
Normal file
23
src/moveit_test/package.xml
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>moveit_test</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>
|
||||||
|
Test for Moveit on IISY
|
||||||
|
</description>
|
||||||
|
<author email="bastian.hofmann@xitaso.com">bastian</author>
|
||||||
|
<maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>moveit_ros_planning_interface</depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
|
||||||
|
</package>
|
||||||
55
src/moveit_test/src/test.cpp
Normal file
55
src/moveit_test/src/test.cpp
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
#include <memory>
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <moveit/move_group_interface/move_group_interface.h>
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
// Initialize ROS and create the Node
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
auto const node = std::make_shared<rclcpp::Node>(
|
||||||
|
"hello_moveit",
|
||||||
|
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
|
||||||
|
);
|
||||||
|
|
||||||
|
// Create a ROS logger
|
||||||
|
auto const logger = rclcpp::get_logger("hello_moveit");
|
||||||
|
|
||||||
|
// Create the MoveIt MoveGroup Interface
|
||||||
|
using moveit::planning_interface::MoveGroupInterface;
|
||||||
|
auto move_group_interface = MoveGroupInterface(node, "iisy");
|
||||||
|
|
||||||
|
// Set a target Pose
|
||||||
|
auto const target_pose = []{
|
||||||
|
geometry_msgs::msg::Pose msg;
|
||||||
|
msg.orientation.w = 1.0;
|
||||||
|
msg.position.x = 0.25;
|
||||||
|
msg.position.y = 0.25;
|
||||||
|
msg.position.z = 1.0;
|
||||||
|
return msg;
|
||||||
|
}();
|
||||||
|
//move_group_interface.setPoseTarget(target_pose);
|
||||||
|
|
||||||
|
std::map<std::string,double> states = {
|
||||||
|
{"base_rot",10.0}
|
||||||
|
};
|
||||||
|
|
||||||
|
move_group_interface.setJointValueTarget(states);
|
||||||
|
|
||||||
|
// Create a plan to that target pose
|
||||||
|
auto const [success, plan] = [&move_group_interface]{
|
||||||
|
moveit::planning_interface::MoveGroupInterface::Plan msg;
|
||||||
|
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
|
||||||
|
return std::make_pair(ok, msg);
|
||||||
|
}();
|
||||||
|
|
||||||
|
// Execute the plan
|
||||||
|
if(success) {
|
||||||
|
move_group_interface.execute(plan);
|
||||||
|
} else {
|
||||||
|
RCLCPP_ERROR(logger, "Planing failed!");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Shutdown ROS
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@ -1 +0,0 @@
|
|||||||
Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb
|
|
||||||
Loading…
x
Reference in New Issue
Block a user