Robot Trick or Treat
by Dr. Yuhan Jiang Oct 29, 2024
Hardware List
NVIDIA Jetson Nano Developer Kit 5V2A USB
Trossenrobotics WidowX 250 S 12V/5A DC
Intel RealSense Depth Camera D415
Rechargeable Battery 12V/5A 5V/2A (Amazon.com)
Any Keyboard (Bluetooth or USB) and Monitor (with HDMI or DP port)
Software List
Ubuntu 20.04, https://github.com/Qengineering/Jetson-Nano-Ubuntu-20-image
ROS2 Galactic, https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros_interface/ros2/software_setup.html#raspberry-pi-4b-arm64-architecture
$ sudo apt install curl
$ curl 'https://raw.githubusercontent.com/Interbotix/interbotix_ros_manipulators/main/interbotix_ros_xsarms/install/rpi4/xsarm_rpi4_install.sh' > xsarm_rpi4_install.sh
$ chmod +x xsarm_rpi4_install.sh
$ ./xsarm_rpi4_install.sh -d galactic
librealsense SDK, https://jetsonhacks.com/2019/12/22/install-realsense-camera-in-5-minutes-jetson-nano/
$ git clone https://github.com/JetsonHacksNano/installLibrealsense
$ cd installLibrealsense
$ ./buildLibrealsense.sh
# if with error of no pyrealsesnse2 module, try uninstall and reinstall pyrealsesnse2
$ pip3 uninstall pyrealsense2
$ pip3 install pyrealsense2
Lunch Robot Arm
ros2 launch interbotix_xsarm_control xsarm_control.launch.py robot_model:=mobile_wx250s
If use the simulator
ros2 launch interbotix_xsarm_control xsarm_control.launch.py robot_model:=mobile_wx250s use_sim:=true
In another terminal
python3 <the following code>.py
Code for Trick or Treat
#!/usr/bin/env python3
import sys
from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS
import numpy as np
import time # Make sure to import the time module
import random # Ensure the random module is imported
import cv2
import pyrealsense2 as rs
def fun(bot): #Cobra model
bot.arm.set_single_joint_position(joint_name='wrist_angle', position=+np.pi / 3)
bot.arm.set_single_joint_position(joint_name='shoulder', position=-1 * np.pi / 10.0)
#bot.arm.set_ee_pose_components(x=0.1, z=0.6, y=0)
#bot.arm.set_single_joint_position(joint_name='waist', position=np.pi / 2.0)
#bot.arm.set_ee_pose_components(x=0.1, z=0.5, y=0)
#bot.arm.set_single_joint_position(joint_name='waist', position=-np.pi / 2.0)
bot.arm.set_ee_pose_components(x=0.1, z=0.6, y=0)
bot.arm.set_single_joint_position(joint_name='waist', position=np.pi / 2.0)
bot.arm.set_ee_pose_components(x=0.1, z=0.5, y=0)
bot.arm.set_single_joint_position(joint_name='waist', position=-np.pi / 2.0)
def getCandy(bot):
bot.arm.set_ee_pose_components(x=0.2, z=0.4)
bot.arm.set_single_joint_position(joint_name='waist', position=-np.pi / 2.0)
bot.arm.set_single_joint_position(joint_name='elbow', position=1 * np.pi / 8.0)
bot.arm.set_single_joint_position(joint_name='wrist_angle', position=+np.pi / 4)
bot.arm.set_single_joint_position(joint_name='shoulder', position=1 * np.pi / 15.0) #
bot.gripper.release()
bot.arm.set_single_joint_position(joint_name='wrist_rotate', position=np.pi / 2)
bot.arm.set_single_joint_position(joint_name='wrist_rotate', position=-1 * np.pi / 2)
bot.arm.set_single_joint_position(joint_name='elbow', position=1 * np.pi / 7.0)
bot.arm.set_single_joint_position(joint_name='wrist_rotate', position=0 * np.pi / 2)
bot.gripper.grasp()
def putCandy(bot,min_dis):
bot.arm.set_single_joint_position(joint_name='wrist_angle', position=+np.pi / 3)
bot.arm.set_single_joint_position(joint_name='shoulder', position=-1 * np.pi / 10.0)
bot.arm.set_ee_pose_components(x=0.3, z=0.4)
while determinDistance() > min_dis:
print('waiting')
#time.sleep(2) # Wait for 2 seconds
bot.gripper.release()
def determinDistance():
frameset = pipe.wait_for_frames()
depth_frame = frameset.get_depth_frame()
depth = np.asanyarray(depth_frame.get_data())
height, width = depth.shape[:2]
expected = 300
aspect = width / height
crop_start = round(expected * (aspect - 1) / 2)
xmin = 0.25
xmax = .75
ymin = 0.00 # upper
ymax = .55
className = 'center'
scale = height / expected
xmin_depth = int((xmin * expected + crop_start) * scale)
ymin_depth = int((ymin * expected) * scale)
xmax_depth = int((xmax * expected + crop_start) * scale)
ymax_depth = int((ymax * expected) * scale)
xmin_depth, ymin_depth, xmax_depth, ymax_depth
# Crop depth data:
depth = depth[xmin_depth:xmax_depth, ymin_depth:ymax_depth].astype(float)
# Get data scale from the device and convert to meters
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
depth = depth * depth_scale
dist, _, _, _ = cv2.mean(depth)
print("Detected a {0} {1:.3} meters away.".format(className, dist))
return dist
def main():
bot = InterbotixManipulatorXS(
robot_model='mobile_wx250s',
group_name='arm',
gripper_name='gripper'
)
if (bot.arm.group_info.num_joints < 5):
bot.core.get_logger().fatal('This demo requires the robot to have at least 5 joints!')
bot.shutdown()
sys.exit()
bot.arm.set_ee_pose_components(x=0.2, z=0.4)
return bot
#bot.shutdown()
if __name__ == '__main__':
bot=main()
# Setup:
pipe = rs.pipeline()
cfg = rs.config()
# cfg.enable_device_from_file("../object_detection.bag")
cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipe.start(cfg)
for i in range(100):
getCandy(bot)
rand_num = random.randint(1, 10)
if rand_num % 2 == 0: # enter Cobra model
fun(bot)
putCandy(bot,0.1)
bot.arm.go_to_sleep_pose()
bot.shutdown()
pipe.stop()