Welcome to Hackster!
Hackster is a community dedicated to learning hardware, from beginner to pro. Join us, it's free!
Elephant Robotics
Published © GPL3+

High-Speed Motion Capture Controller to Remote Control Robot

The myController S570 has high data acquisition speed and remote control ability, greatly simplifying the programming of humanoid robots.

BeginnerFull instructions provided1 hour616

Things used in this project

Hand tools and fabrication machines

myController S570
Elephant Robotics myController S570

Story

Read more

Schematics

_20250110150547_h2TT5V0vxV.png

Code

control_dual_ur5.launch

XML
<launch>
  <arg name="model" ></arg>

  <include file = "$(find dual_ur_description)/launch/load_dual_ur5.launch"></include>

  <node
    name="joint_state_publisher_gui"
    pkg="joint_state_publisher_gui"
    type="joint_state_publisher_gui" ></node>

  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="robot_state_publisher" ></node>

    <!-- start RViz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find dual_ur_description)/rviz/visualize_urdf.rviz" required="true"></node>

    <!-- transform of joints -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world dual_base"></node>

</launch>

test.py

Python
#!/usr/bin/env python

"""
This package need `pymycobot`.
This file for test the API if right.

Just can run in Linux.
"""
from exoskeleton_api import Exoskeleton, ExoskeletonSocket
import rospy
from math import pi
import time
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
import rosnode
import os
import subprocess

os.system("sudo chmod 777 /dev/ttyACM*")

obj = Exoskeleton(port="/dev/ttyACM3")

def shutdown_ros_node(node_name):
    try:
        subprocess.run(['rosnode', 'kill', node_name])
        print(f"Node {node_name} has been shutdown.")
    except subprocess.CalledProcessError as e:
        print(f"Error: {e}")

# init ROS node
rospy.init_node('dual_arm_joint_state_publisher')

shutdown_ros_node('joint_state_publisher_gui')

# create publisher
pub = rospy.Publisher('/joint_states', JointState, queue_size=10)

rate = rospy.Rate(100) # 100Hz

joint_state = JointState()

while not rospy.is_shutdown():
    joint_state.header = Header()
    
    joint_state.header.stamp = rospy.Time.now()
    
    # origin
    #joint_state.name = ['joint1', 'joint2', 'joint3','joint4', 'joint5', 'joint6','joint7','joint8','joint9','joint10',
    #                    'joint11','joint12','joint13','joint14']
    # new for dual-arm
    joint_state.name =[
            'arm_0_shoulder_pan_joint', 
            'arm_0_shoulder_lift_joint', 
            'joint3',
            'arm_0_elbow_joint', 
            'arm_0_wrist_2_joint',
            'arm_0_wrist_1_joint', 
            'arm_0_wrist_3_joint', 
            'arm_1_shoulder_pan_joint', 
            'arm_1_shoulder_lift_joint', 
            'joint10', 
            'arm_1_elbow_joint',
            'arm_1_wrist_2_joint', 
            'arm_1_wrist_1_joint', 
            'arm_1_wrist_3_joint',
        ]
    l_angle = obj.get_arm_data(1)
    l_angle = l_angle[:7]
    l_angle = [int(x) for x in l_angle]
    print("l:",l_angle)
    r_angle = obj.get_arm_data(2)
    r_angle = r_angle[:7]
    r_angle = [int(y) for y in r_angle]
    print("r:",r_angle)
    angles = l_angle + r_angle 

    angle = [a/180*pi for a in angles]

    # Rotation relation mapping from exoskeleton to robot joint
    angle[0]-=3.14
    angle[3]+=0.52
    angle[4]-=1.57


    angle[10]+=0.52
    angle[11]-=1.57
    angle[12]=-angle[12]-1.57


    
    joint_state.position = angle
    joint_state.effort = []
    joint_state.velocity = []

    pub.publish(joint_state)
    rospy.loginfo('successfully published')
    rate.sleep()

Credits

Elephant Robotics
101 projects • 216 followers
An Innovative Robotics Company.
Contact

Comments

Please log in or sign up to comment.

Related channels and tags
  • Elephant Robotics
  • Robotics
  • Robotics