Example Code¶
This section presents example code snippets that demonstrate key programming concepts and techniques. Each example serves as a practical guide, helping to apply theoretical knowledge in real-world scenarios. Whether you are a novice building a foundation or an experienced developer refining your skills, these examples provide valuable insights and clear, actionable steps.
Before getting started, make sure that you are already familiar with the workflow of each SDK.
Human Class¶
Please refer to Quick Start for detailed code.
EndEffector Class¶
import time
import json
import numpy as np
from rocs_client import EndEffector, EndEffectorScheme
async def on_connected():
print("WebSocket opened...")
async def on_message(message: str):
print("Received message:", message)
message_data = json.loads(message)
global flag, start_point
if 'data' in message_data and 'hand' in message_data['data'] and 'data' in message_data['data']['hand']:
if flag == 0:
right_x_value = message_data['data']['hand']['data']['right_x']
right_y_value = message_data['data']['hand']['data']['right_y']
right_z_value = message_data['data']['hand']['data']['right_z']
start_point = np.array([right_x_value, right_y_value, right_z_value])
flag = 1
async def on_close():
print("WebSocket closed")
async def on_error(error: Exception):
print("WebSocket error:", error)
def generate_trajectory(start_point, end_point, step_size=0.01):
distance = np.linalg.norm(end_point - start_point)
direction = (end_point - start_point) / distance
num_points = int(distance / step_size)
trajectory_points = []
for i in range(num_points):
displacement = i * step_size * direction
current_point = start_point + displacement
trajectory_points.append(current_point)
return trajectory_points
def smooth_move_right(x:float, y:float, z:float):
global start_point
end_point = np.array([x, y, z])
print("end point", start_point)
trajectory = generate_trajectory(start_point, end_point)
for point in trajectory:
print(point[0],point[1],point[2])
end_effector.control_right(EndEffectorScheme(x=point[0], y=point[1], z=point[2], qx=0, qy=0, qz=0, qw=0))
time.sleep(0.05)
end_effector.control_right(EndEffectorScheme(x=x, y=y, z=z, qx=0, qy=0, qz=0, qw=0))
start_point = end_point
end_effector = EndEffector(host="192.168.12.1", on_connected=on_connected, on_message=on_message, on_close=on_close, on_error=on_error)
start_point = np.array([0, 0, 0])
flag = 0
input("Enabling endeffector mode. Press any key to continue...")
result = end_effector.enable()
print(f'test_enable: {result}')
time.sleep(5)
position = end_effector._send_request(url='/robot/end_effector/origin_point', method="GET")
time.sleep(1)
input("Type in 'close' to end Endeffector input. Press any key to continue...")
while True:
user_input = input("Enter three float numbers separated by spaces: ")
if user_input == "close":
break
else:
coordinate_x, coordinate_y, coordinate_z = user_input.split()
print(coordinate_x, coordinate_y, coordinate_z)
smooth_move_right( float(coordinate_x) , float(coordinate_y), float(coordinate_z))
end_effector.exit()
Motor Class¶
import time
import math
from rocs_client import Motor
import threading
import time
def smooth_motion_by_interpolation(no, orientation, target_angle, offset=0.05, interval=0.004):
"""
Use differential to move the motor smoothly
Args:
no: Number of the motor to be operated
orientation: Orientation of the motor to be operated
target_angle: Angle of motion
offset: The Angle of each move
interval: Interval of differential
"""
if int(no) > 8:
print('Motor number greater than 8 is not supported.')
return
def wait_target_done(rel_tol=2):
while True:
try:
p = motor.get_motor_pvc(no, orientation)['data']['position']
print(f'============={p}')
if math.isclose(p, target_angle, rel_tol=rel_tol):
break
except Exception as e:
print(f'wait_target_done err: {e}')
while True:
try:
result = motor.get_motor_pvc(no, orientation)
current_position = result['data']['position']
if current_position is not None:
break
except Exception as e:
print(f'current_position err: {e}')
target_position = target_angle
cycle = abs(int((target_position - current_position) / offset))
for i in range(0, cycle):
if target_position > current_position:
current_position += offset
else:
current_position -= offset
motor.move_motor(no, orientation, current_position)
time.sleep(interval)
wait_target_done()
def action_hug():
"""
In this example, both the left arm and the right arm move to the same position simultaneously
using two threads to distribute the movement.
"""
def left():
smooth_motion_by_interpolation('1', 'left', 30, 0.3, 0.005)
smooth_motion_by_interpolation('2', 'left', -60, 0.3, 0.005)
smooth_motion_by_interpolation('4', 'left', 60, 0.3, 0.005)
smooth_motion_by_interpolation('1', 'left', 45, 0.3, 0.005)
def right():
smooth_motion_by_interpolation('1', 'right', -30, 0.3, 0.005)
smooth_motion_by_interpolation('2', 'right', 60, 0.3, 0.005)
smooth_motion_by_interpolation('4', 'right', -60, 0.3, 0.005)
smooth_motion_by_interpolation('1', 'right', -45, 0.3, 0.005)
left = threading.Thread(target=left)
right = threading.Thread(target=right)
left.start(), right.start()
left.join(), right.join()
motor = Motor(host="192.168.12.1")
arm_motor = motor.limits[0:17] if len(motor.limits) > 18 else []
clamping_jaw = motor.limits[17:19] if len(motor.limits) > 20 else []
dexterous_hand = motor.limits[19:31] if len(motor.limits) > 32 else []
print(f'arm_motor: {arm_motor}')
print(f'clamping_jaw: {clamping_jaw}')
print(f'dexterous_hand: {dexterous_hand}')
motors = arm_motor + clamping_jaw
time.sleep(3)
input("Motor PD flag setting. Press any key to continue...")
for item in motors:
motor.set_motor_pd_flag(item['no'], item['orientation'])
input("Motor PD setting. Press any key to continue... ")
for item in motors:
motor.set_motor_pd(item['no'], item['orientation'], 0.36, 0.042)
input("Enabling all motors. Press any key to continue... ")
for item in motors:
motor.enable_motor(item['no'], item['orientation'])
input("Reading PVC for left shoulder roll motor. Press any key to continue...")
print(f"test_get_pvc {motor.get_motor_pvc('2', 'left')}")
input("Smooth move for left shoulder roll motor(outward). Press any key to continue...")
smooth_motion_by_interpolation('2', 'left', -40)
input("Direct move for left shoulder pitch motor(forward). Press any key to continue...")
motor.move_motor('1', 'left', 10)
input("Direct move for left shoulder pitch motor(back). Press any key to continue...")
motor.move_motor('1', 'left', 0)
input("Smooth move for left shoulder roll motor(back). Press any key to continue...")
smooth_motion_by_interpolation('2', 'left', 0)
input("Testing hug action. Press any key to continue...")
action_hug()
input("Exiting...")
motor.exit()