ADD ISAAC SIM package

Author: Wonikrobotics-git

README.md

@@ -30,10 +30,27 @@ These packages are tested on ROS2 Humble(Ubuntu 22.04). It will likely not work
 - allegro_hand_driver : Main driver for sending and receiving data with the Allegro Hand.
   	- CANAPI : Drivers for CAN communication.
   	- rs485 : Drivers for RS-485 communication.
+- allegro_hand_isaacsim : Node that receives position datas from ISAAC SIM, sorts them in correct order and resends the datas to REAL Allegro Hand.
 - allegro_hand_keyboard : Node that sends the command to control Allegro Hand. All commands need to be pre-defined.
 - allegro_hand_moveit : Provide MOVEIT2 package for Allegro Hand V5-3Finger.
 - allegro_hand_gui : Node that control the allegro hand with gui program.
 - bhand : Library files for the predefined grasps and actions., available on 64 bit versions.
+  - ⚠ **Default: x86-64-bit.** If using a **ARM64**, update the symlink accordingly from [here](https://github.com/Wonikrobotics-git/Bhandlib_ARM).
+ 
+## Topic description
+
+- Control
+	- /allegroHand_(NUM)/lib_cmd :  Hand command.
+ 	- /allegroHand_(NUM)/joint_cmd : Desired hand joint positions and control REAL Allegro Hand.
+  	- /allegroHand_(NUM)/force_chg : Change grasp force of grasping algrotihm(grasp_3).
+  	- /allegroHand_(NUM)/time_chg : Change time of moving target positions of each joints.
+  	- /allegroHand_(NUM)/envelop_torque : Change torque of envelop command.
+- Joint States
+  	- /allegroHand_(NUM)/joint_states : REAL Allegro Hand current joint positions.
+  	- /allegroHand_sim/joint_states : ISAAC SIM Allegro Hand current joint positions.
+- Sensors
+  	- /allegroHand_(NUM)/tactile_sensors : REAL tactile sensors data { Finger1 , Finger2 , Finger3 }.
+  	- /allegroHand_sim/contact_sensors : ISAAC SIM contact sensors data { Finger1 , Finger2 , Finger3 }.
 
 ## Install the PCAN driver
 
@@ -63,9 +80,6 @@ mkdir allegro_ws
 sudo apt-get update
 sudo apt-get install ros-<distro>-xacro
 sudo apt install ros-humble-moveit
-sudo apt install ros-<distro>-controller-manager
-sudo apt install ros-<distro>-joint-state-broadcaster
-sudo apt install ros-<distro>-joint-trajectory-controller
 ~~~
 
 3. Download ROS2 package for Allegro Hand V5 using below command.
@@ -113,6 +127,7 @@ VISUALIZE:=true|false (default is false)
 MOVEIT:=true|false (default is false)
 GUI:=true|false (default is false)
 RS485:=true|false (default is false)
+ISAAC:=true|false (default is false)
 ~~~
 
 - If you want to visualize Allegro Hand on Rviz2:
@@ -133,7 +148,12 @@ ros2 launch allegro_hand_controllers allegro_hand.launch.py GUI:=true
 - If you want to control Allegro Hand through RS-485:
 ~~~bash
 ros2 launch allegro_hand_controllers allegro_hand.launch.py RS485:=true
-~~~  
+~~~
+
+- If you want to use Allegro Hand with ISAAC SIM SIM2REAL:
+~~~bash
+ros2 launch allegro_hand_controllers allegro_hand.launch.py ISAAC:=true
+~~~
 
 ## Control more than one hand
 
@@ -195,6 +215,14 @@ ros2 run allegro_hand_keyboards allegro_hand_keyboard --ros-args allegroHand_0/l
 ~~~
 
 **These are example commands.You may need to change CAN_DEVICE, PORT and NUM arguments accroding to your system.**
+
+## ISAACSIM
+
+We now introuce two features —REAL2SIM and SIM2REAL—that integrate the Allegro Hand V5 with Isaac Sim.
+
+Please refer to the following guide for detailed information : [allegro_hand_isaacsim](https://github.com/Wonikrobotics-git/private_allegro_ros2_v5/tree/main/3finger/src/allegro_hand_isaacsim)
+
+
 ## MOVEIT2 
 
 Please refer to the ROS1 manual for guidance.
@@ -207,3 +235,4 @@ You can find installation guide in here : [MOVEIT2](https://moveit.picknik.ai/ma
 
 These newly added feature function identically to their ROS1 counterparts. Please refer to the ROS1 manual for guidance.
 Our latest Allegro Hand V5 ROS1 package : [ROS1](https://github.com/Wonikrobotics-git/allegro_hand_ros_v5-3Finger)
+

src/allegro_hand_controllers/launch/allegro_hand.launch.py

@@ -30,6 +30,12 @@ def generate_launch_description():
         description='Flag to enable/disable GUI'
     )
 
+    declare_sim_arg = DeclareLaunchArgument(
+        'ISAAC',
+        default_value='false',
+        description='Flag to enable/disable ISAAC SIM2REAL'
+    )
+
     declare_polling_arg = DeclareLaunchArgument(
         'POLLING',
         default_value='true',
@@ -96,10 +102,11 @@ def setup_can(context):
         declare_polling_arg,
         declare_can_device_arg,
         declare_num_arg,
-		declare_moveit_arg,
-		declare_gui_arg,
-		declare_rs485_arg,
-		declare_port_arg,
+        declare_moveit_arg,
+        declare_gui_arg,
+        declare_rs485_arg,
+        declare_port_arg,
+        declare_sim_arg,
         OpaqueFunction(function=setup_can,
 					   condition=UnlessCondition(LaunchConfiguration('RS485'))),
         Node(
@@ -128,7 +135,8 @@ def setup_can(context):
         ),		
         Node(
             package='robot_state_publisher',
-            output='screen',
+            output='log',
+            arguments=['--ros-args', '--log-level', 'WARN'],
             executable='robot_state_publisher',
             parameters=[{'robot_description': Command(['xacro ', urdf_path])}],
             remappings=[
@@ -156,6 +164,13 @@ def setup_can(context):
 				        ('forcechange',PythonExpression(["'allegroHand_",LaunchConfiguration('NUM'),"/force_chg'"])),
 				        ('timechange',PythonExpression(["'allegroHand_",LaunchConfiguration('NUM'),"/time_chg'"]))],		
             condition=IfCondition(LaunchConfiguration('GUI'))
+        ),
+        Node(
+            package='allegro_hand_isaacsim',
+            executable='allegro_hand_sim2real',
+            output='screen',
+			remappings=[('allegroHand/joint_cmd',PythonExpression(["'allegroHand_",LaunchConfiguration('NUM'),"/joint_cmd'"]))],		
+            condition=IfCondition(LaunchConfiguration('ISAAC'))
         )
 
     ])

src/allegro_hand_controllers/src/allegro_node.cpp

@@ -58,6 +58,8 @@ AllegroNode::AllegroNode(const std::string nodeName, bool sim /* = false */)
   // Advertise current joint state publisher and subscribe to desired joint
   // states.
   joint_state_pub = this->create_publisher<sensor_msgs::msg::JointState>(JOINT_STATE_TOPIC, 3);
+  tactile_sensor_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>(TACTILE_SENSOR_TOPIC, 10);   
+
   joint_cmd_sub = this->create_subscription<sensor_msgs::msg::JointState>(DESIRED_STATE_TOPIC, 1, // queue size
                                  std::bind(&AllegroNode::desiredStateCallback, this, std::placeholders::_1));
   time_sub = this->create_subscription<std_msgs::msg::Float32>("timechange", 1, // queue size
@@ -98,6 +100,8 @@ void AllegroNode::publishData() {
     current_joint_state.velocity[i] = current_velocity[i];
     current_joint_state.effort[i] = desired_torque[i];
   }
+  tactile_sensor.data = std::vector<int>(std::begin(fingertip_sensor), std::end(fingertip_sensor));
+  tactile_sensor_pub->publish(tactile_sensor);
   joint_state_pub->publish(current_joint_state);
 }
 
@@ -128,6 +132,13 @@ void AllegroNode::updateController() {
       // update joint positions:
       canDevice->getJointInfo(current_position);
 
+      for(int i =0; i<9; i++){
+          if(current_position[i] < -3.14)
+            current_position[i] += 3.14;
+          if(current_position[i] > 3.14)
+            current_position[i] -= 3.14;
+      }
+
       OperatingMode = 0;
 
       if (OperatingMode == 0) {
@@ -137,7 +148,7 @@ void AllegroNode::updateController() {
           f[0] = f[1] = f[2] = 5.0f;
       }
 
-
+     // std::cout << current_position[1]*180/3.141592 <<" "<< current_position[4]*180/3.141592<<" "<<current_position[7]*180/3.141592<<" "<<std::endl;
       // calculate control torque:
       computeDesiredTorque();
 

src/allegro_hand_controllers/src/allegro_node.h

@@ -15,9 +15,10 @@ using namespace allegro;
 
 #include "sensor_msgs/msg/joint_state.hpp"
 #include "std_msgs/msg/string.hpp"
-#include "std_msgs/msg/u_int8_multi_array.hpp"
+#include "std_msgs/msg/int32_multi_array.hpp"
 #include "bhand/BHand.h"
 #include <std_msgs/msg/float32.hpp>
+#include <vector>
 
 // Forward declaration.
 class AllegroHandDrv;
@@ -28,6 +29,7 @@ class AllegroHandDrv;
 const std::string JOINT_STATE_TOPIC = "allegroHand/joint_states";
 const std::string DESIRED_STATE_TOPIC = "allegroHand/joint_cmd";
 const std::string LIB_CMD_TOPIC = "allegroHand/lib_cmd";
+const std::string TACTILE_SENSOR_TOPIC = "allegroHand/tactile_sensors";
 
 class AllegroNode: public rclcpp::Node {
  public:
@@ -77,6 +79,7 @@ class AllegroNode: public rclcpp::Node {
 
   // ROS stuff
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub;
+  rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr tactile_sensor_pub; 
   rclcpp::Time t_last_pub;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_cmd_sub;
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr time_sub;
@@ -87,6 +90,8 @@ class AllegroNode: public rclcpp::Node {
   sensor_msgs::msg::JointState current_joint_state;
   sensor_msgs::msg::JointState desired_joint_state;
 
+  std_msgs::msg::Int32MultiArray tactile_sensor;
+
   // ROS Time
   rclcpp::Time tstart;
   rclcpp::Time tnow;

src/allegro_hand_controllers/urdf/allegro_hand_description_3f/allegro_hand_description_3f.usd

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(allegro_hand_isaacsim)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+
+add_executable(allegro_hand_sim2real src/sim2real.cpp)
+
+ament_target_dependencies(allegro_hand_sim2real
+  rclcpp
+  sensor_msgs
+)
+
+install(TARGETS
+  allegro_hand_sim2real
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/allegro_hand_controllers/urdf/allegro_hand_description_3f/configuration/allegro_hand_description_3f_base.usd

@@ -0,0 +1,27 @@
+# allegro_hand_isaacsim_package
+
+
+
+# Setup
+
+## REAL2SIM
+
+|**Instruction**|**Figure**|
+|----------------------------------------------|----------------------|
+| 1. Launch Isaac Sim & Check Extenstions(ROS2 Bridge)   | <img width="100%" height="100%" src="./asset/Check_extensions.png"> | 
+| 2. Open AH_V5(3F)_REAL2SIM.usd file & Select ‘Yes’ on the warning message. | <img width="100%" height="100%" src="./asset/REAL2SIM_warning.png"> | 
+| 3. Check Action Graph : <br> - **ROS Subscriber** <br> topic name : allegroHand_0/joint_states <br> - **Articulation Controller** <br> target prim : /World/allegro_hand | <img width="100%" height="100%" src="./asset/ROS_subscribe_joint_states.png"> |
+| 4. Launch Allegro Hand V5 controller node |``` ros2 launch allegro_hand_controllers allegro_hand.launch.py ``` |
+| 5. Press Play Button &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; &nbsp; &nbsp; &nbsp; &nbsp;| <img   width="100%" height="100%" src="./asset/REAL2SIM_play_3F.png">|
+
+
+## SIM2REAL
+
+|**Instruction**|**Figure**|
+|------------------------|----------------------|
+| 1. Launch Isaac Sim & Check Extenstions(ROS2 Bridge)  | <img width="100%" height="100%" src="./asset/Check_extensions.png"> | 
+| 2. Open AH_V5(3F)_SIM2REAL.usd file| <img width="100%" height="100%" src="./asset/SIM2REAL.png"> | 
+| 3. Check Action Graph : <br> - **ROS Publisher(Joint states)** <br> topic name : allegroHand_sim/joint_states <br> targetPrim : /World/allegro_hand/root_joint <br> - **ROS Publisher(Contact sensor)** <br> topic name : allegroHand_sim/contact_sensors <br> - **Articulation Controller** <br> target prim : /World/allegro_hand/root_joint | <img width="100%" height="100%" src="./asset/SIM2REAL_actiongraph.png"> |
+| 4. Launch Allegro Hand V5 controller node with Isaac arguments | ``` ros2 launch allegro_hand_controllers allegro_hand.launch.py ISAAC:=true ``` |
+| 5. Press Play Button & Control the hand joint changing joint_positions_array of ActionGraph| <img   width="100%" height="100%" src="./asset/SIM2REAL_play_3F.png"> |
+