I want to open / close simple robotiq_hande_gripper, so I created this USD file, but I can’t control the gripper py publishing the joint states, can you please tell me how to fix it? thanks.
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import numpy as np
import time
class TestROS2Bridge(Node):
def __init__(self, jnts_topic: str, include_gripper: bool = False):
super().__init__("test_ros2bridge")
self._include_gripper = include_gripper
# Create the publisher. This publisher will publish a JointState message to the /joint_command topic.
self.publisher_ = self.create_publisher(JointState, jnts_topic, 10)
# Create a JointState message
self.joint_state = JointState()
self.joint_state.name.append("robotiq_hande_left_finger_joint")
self.joint_state.name.append("robotiq_hande_right_finger_joint")
# Initialize joint positions (for robot and gripper)
self.joint_state.position = np.zeros(len(self.joint_state.name), dtype=np.float64).tolist()
# Store the default positions
self.gripper_default = np.array([0.0, 0.0]) # Default gripper joint positions (if included)
self.max_joints = np.array([0.025, -0.05])
self.min_joints = np.array([0.0, 0.003])
# Track the time for sin-based movement of joints
self.time_start = time.time()
# Create a timer to periodically send joint commands
timer_period = 0.05 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
# Set timestamp
self.joint_state.header.stamp = self.get_clock().now().to_msg()
# Compute joint positions (robot and gripper)
joint_position = (
np.sin(time.time() - self.time_start)
* (self.max_joints - self.min_joints)
* 0.5
+ self.gripper_default
)
# Make sure gripper joints are within the allowed range
joint_position[-2:] = np.clip(joint_position[-2:], self.min_joints[-2:], self.max_joints[-2:])
# Update the joint positions
self.joint_state.position = joint_position.tolist()
# Publish the JointState message
self.publisher_.publish(self.joint_state)
def main(args=None):
jnts_command_topic = "/isaac/joint_command"
include_gripper_ = True # Set to True if you want to include gripper joints
rclpy.init(args=args)
# Initialize the ROS2 node with the option to include gripper joints
ros2_publisher = TestROS2Bridge(
jnts_topic=jnts_command_topic,
include_gripper=include_gripper_
)
rclpy.spin(ros2_publisher)
# Destroy the node explicitly
ros2_publisher.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
robotiq_hande_w_act_graph.zip (2.9 MB)
Isaac Sim Version
4.5.0
Operating System
Ubuntu 22.04
GPU Information
- Model: GeForce RTX 2070
- Driver Version: 550.144.03
robotiq_hande_w_act_graph.zip (2.9 MB)