Elephant Robotics
Published © GPL3+

Folding Clothes with a Humanoid Robot and Exoskeleton

A technical case that demonstrates how a wearable exoskeleton controller captures human arm joint data and maps it to a humanoid robot.

BeginnerFull instructions provided3 hours8

Things used in this project

Hardware components

Mercury X1
Elephant Robotics Mercury X1
×1

Hand tools and fabrication machines

myController S570
Elephant Robotics myController S570

Story

Read more

Code

remote_contrpl.py

Python
import threading
from pymycobot import Mercury
import time
from pymycobot import Exoskeleton
import os

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


# before using the code  check the myController port name by "ls /dev/ttyACM*"
obj = Exoskeleton(port="/dev/ttyACM4")

ml = Mercury("/dev/left_arm")
mr = Mercury("/dev/right_arm")

l_control_mode = 1
r_control_mode = 1

l_last_mode = 0
r_last_mode = 0

# set control type
ml.set_movement_type(3)
mr.set_movement_type(3)
ml.set_vr_mode(1)
mr.set_vr_mode(1)


def jointlimit_left(angles):
    max = [15, 70, 40, -20, 175, 220, 200]
    min = [-15, 10, -40, -70, -175, 80, 50]
    for i in range(7):
        if angles[i] > max[i]:
            angles[i] = max[i]
        if angles[i] < min[i]:
            angles[i] = min[i]
            
def jointlimit_right(angles):
    max = [45, 70, 40, -20, 175, 180, 180]
    min = [-15, 10, -40, -70, -175, 80, 30]
    for i in range(7):
        if angles[i] > max[i]:

            angles[i] = max[i]
        if angles[i] < min[i]:
            angles[i] = min[i]        


def gripper_control_open(mc):
    mc.set_pro_gripper_angle(14, 80)


def gripper_control_close(mc):
    mc.set_pro_gripper_angle(14, 0)


# 1 left arm ,2  right arm
def control_arm(arm):
    global l_control_mode, l_last_mode, r_control_mode, r_last_mode
    st = 0
    while True:
        try:
            if arm == 1:
                arm_data = obj.get_arm_data(1)
                #print("l: ", arm_data)
                mc = ml
            elif arm == 2:
                arm_data = obj.get_arm_data(2)
                #print("r: ", arm_data)
                mc = mr
            else:
                raise ValueError("error arm")

            
            time_err = 1000 * (time.time() - st)
            st = time.time()
            if arm == 1: #left arm
            #align the robot - angle - myController relationship
                mercury_list = [
                arm_data[0], -arm_data[1], arm_data[2], -arm_data[3], arm_data[4],240 + arm_data[5]*0.5, arm_data[6] + 160]
            # limit joint angle 
                jointlimit_left(mercury_list)
                if arm_data[7] == 0 and l_last_mode == 0:
                    print(6)
                    l_last_mode = 1
                    l_control_mode += 1
                    if l_control_mode > 3:
                        l_control_mode = 1

                    if l_control_mode == 1:
                        obj.set_color(arm, 0, 255, 0)
                    elif l_control_mode == 2:
                        obj.set_color(arm, 0, 0, 255)
                    else:
                        obj.set_color(arm, 255, 0, 0)

                if arm_data[7] == 1:
                    l_last_mode = 0

                if l_control_mode == 1:
                    TI = 10
                elif l_control_mode == 2:
                    TI = 5
                else:
                    TI = 3
                if arm_data[9] == 0:
                    threading.Thread(target=gripper_control_close, args=(mc,)).start()
                    print("===== gripper close ============")

                elif arm_data[10] == 0:
                    threading.Thread(target=gripper_control_open, args=(mc,)).start()
                    print("===== gripper open ============")

            else:  #  right arm
                # align the robot pose by testing ( you can modify by yourself again )
                mercury_list = [
                arm_data[0], -arm_data[1], arm_data[2], -arm_data[3], arm_data[4],200 + arm_data[5]*0.8, arm_data[6] + 80]

                # limit the joint angle  ( you can modify by yourself again )
                jointlimit_right(mercury_list)
                if arm_data[7] == 0 and r_last_mode == 0:
                    print(6)
                    r_last_mode = 1
                    r_control_mode += 1
                    if r_control_mode > 3:
                        r_control_mode = 1
                    if r_control_mode == 1:
                        obj.set_color(arm, 0, 255, 0)
                    elif r_control_mode == 2:
                        obj.set_color(arm, 0, 0, 255)
                    else:
                        obj.set_color(arm, 255, 0, 0)

                if arm_data[7] == 1:
                    r_last_mode = 0
                if r_control_mode == 1:
                    TI = 10
                elif r_control_mode == 2:
                    TI = 5
                else:
                    TI = 3

                if arm_data[9] == 0:
                    threading.Thread(target=gripper_control_close, args=(mc,)).start()
                    print("===== gripper close ============")

                elif arm_data[10] == 0:
                    threading.Thread(target=gripper_control_open, args=(mc,)).start()
                    print("===== gripper open ============")

                if arm_data[9] == 0 and arm_data[10] == 0:
                    time.sleep(0.01)
                    continue
            print(arm)
            print(mercury_list)
            mc.send_angles(mercury_list, TI, _async=True)
            time.sleep(0.01)

        except Exception as e:
            print(e)
            pass

time.sleep(10)
# left arm
threading.Thread(target=control_arm, args=(1,)).start()
# right arm
threading.Thread(target=control_arm, args=(2,)).start()

Credits

Elephant Robotics
112 projects • 236 followers
An Innovative Robotics Company.

Comments