multi panda sim initial commit

smacc2_sm_reference_library/sm_multi_panda_sim/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.5)
+project(sm_multi_panda_sim)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+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(ros_timer_client REQUIRED)
+find_package(smacc2 REQUIRED)
+find_package(Boost COMPONENTS thread REQUIRED)
+find_package(move_group_interface_client REQUIRED)
+
+find_package(sr_all_events_go REQUIRED)
+find_package(ros_publisher_client REQUIRED)
+find_package(keyboard_client REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  ros_timer_client
+  smacc2
+  move_group_interface_client
+  sr_all_events_go
+  ros_publisher_client
+  keyboard_client
+)
+
+include_directories(
+  include
+  ${smacc2_INCLUDE_DIRS}
+  ${move_group_interface_client_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node
+  src/${PROJECT_NAME}/${PROJECT_NAME}_node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${smacc2_LIBRARIES}
+  ${ros_timer_client_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${move_group_interface_client_LIBRARIES}
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  config
+  launch
+  rviz
+  # urdf
+  # srdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(TARGETS
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

smacc2_sm_reference_library/sm_multi_panda_sim/README.md

@@ -0,0 +1,42 @@
+ <h2>Build Instructions</h2>
+
+First, source your chosen ros2 distro.
+```
+source /opt/ros/rolling/setup.bash
+```
+```
+source /opt/ros/galactic/setup.bash
+```
+
+Before you build, make sure you've installed all the dependencies...
+
+```
+rosdep install --ignore-src --from-paths src -y -r
+```
+
+Then build with colcon build...
+
+```
+colcon build
+```
+  <h2>Operating Instructions</h2>
+After you build, remember to source the proper install folder...
+
+```
+source ~/<ros2_ws>/install/setup.bash
+```
+
+And then run the launch file...
+
+```
+ros2 launch sm_multi_panda_sim sm_multi_panda_sim.launch
+```
+
+ <h2>Viewer Instructions</h2>
+If you have the SMACC2 Runtime Analyzer installed then type...
+
+```
+ros2 run smacc2_rta smacc2_rta
+```
+
+If you don't have the SMACC2 Runtime Analyzer click <a href="https://robosoft.ai/product-category/smacc2-runtime-analyzer/">here</a>

smacc2_sm_reference_library/sm_multi_panda_sim/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

smacc2_sm_reference_library/sm_multi_panda_sim/config/hand.xacro

@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:macro name="hand">
+    <!--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="hand">
+      <link name="panda_hand" />
+      <link name="panda_leftfinger" />
+      <link name="panda_rightfinger" />
+      <joint name="panda_finger_joint1" />
+      <passive_joint name="panda_finger_joint2" />
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <!--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="panda_hand" link2="panda_leftfinger" reason="Adjacent" />
+    <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent" />
+    <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default" />
+  </xacro:macro>
+</robot>

smacc2_sm_reference_library/sm_multi_panda_sim/config/joint_limits.yaml

@@ -0,0 +1,55 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# 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]
+
+# As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee
+# that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel
+# to max accel in 1 ms) the acceleration limits are the ones that satisfy
+# max_jerk = (max_acceleration - min_acceleration) / 0.001
+
+joint_limits:
+  panda_joint1:
+    has_velocity_limits: true
+    max_velocity: 2.1750
+    has_acceleration_limits: true
+    max_acceleration: 3.75
+  panda_joint2:
+    has_velocity_limits: true
+    max_velocity: 2.1750
+    has_acceleration_limits: true
+    max_acceleration: 1.875
+  panda_joint3:
+    has_velocity_limits: true
+    max_velocity: 2.1750
+    has_acceleration_limits: true
+    max_acceleration: 2.5
+  panda_joint4:
+    has_velocity_limits: true
+    max_velocity: 2.1750
+    has_acceleration_limits: true
+    max_acceleration: 3.125
+  panda_joint5:
+    has_velocity_limits: true
+    max_velocity: 2.6100
+    has_acceleration_limits: true
+    max_acceleration: 3.75
+  panda_joint6:
+    has_velocity_limits: true
+    max_velocity: 2.6100
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  panda_joint7:
+    has_velocity_limits: true
+    max_velocity: 2.6100
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  panda_finger_joint1:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: false
+    max_acceleration: 0.0
+  panda_finger_joint2:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: false
+    max_acceleration: 0.0

smacc2_sm_reference_library/sm_multi_panda_sim/config/kinematics.yaml

@@ -0,0 +1,8 @@
+left_panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+#  kinematics_solver_timeout: 0.05
+right_panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+#  kinematics_solver_timeout: 0.05

smacc2_sm_reference_library/sm_multi_panda_sim/config/left_initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for the panda arm's ros2_control fake system
+initial_positions:
+  panda_joint1: 0.0
+  panda_joint2: -0.785
+  panda_joint3: 0.0
+  panda_joint4: -2.356
+  panda_joint5: 0.0
+  panda_joint6: 1.571
+  panda_joint7: 0.785

smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_controllers.yaml

@@ -0,0 +1,7 @@
+# MoveIt uses this configuration for controller management
+trajectory_execution:
+  allowed_execution_duration_scaling: 1.2
+  allowed_goal_duration_margin: 0.5
+  allowed_start_tolerance: 0.01
+
+moveit_controller_manager: moveit_ros_control_interface/MoveItMultiControllerManager

smacc2_sm_reference_library/sm_multi_panda_sim/config/moveit_cpp.yaml

@@ -0,0 +1,18 @@
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
+  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
+  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  #namespace: "moveit_cpp"  # optional, default is ~
+  pipeline_names: ["ompl"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 1.0
+  max_acceleration_scaling_factor: 1.0