self.circles[0][1] = mouse_y
# create our interface
interface = PyGame(robot_config, arm_sim, on_click=on_click)
interface.connect()
ctrlr = OSC(robot_config, kp=20, vmax=10)
avoid = signals.AvoidObstacles(robot_config, threshold=1)
# create an obstacle
interface.add_circle(xyz=[0, 0, 0], radius=.2)
# create a target
target_xyz = [0, 2, 0]
interface.set_target(target_xyz)
# set up lists for tracking data
ee_path = []
target_path = []
try:
# run ctrl.generate once to load all functions
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(q=zeros, dq=zeros, target_pos=target_xyz)
robot_config.orientation('EE', q=zeros)
print('\nSimulation starting...\n')
print('\nClick to move the obstacle.\n')
while 1:
# get arm feedback
self.adaptation = not self.adaptation
print('adaptation: ', self.adaptation)
# create our interface
interface = PyGame(robot_config,
arm_sim,
on_click=on_click,
on_keypress=on_keypress)
interface.connect()
interface.adaptation = False # set adaptation False to start
# create a target
feedback = interface.get_feedback()
target_xyz = robot_config.Tx('EE', feedback['q'])
interface.set_target(target_xyz)
# set up lists for tracking data
ee_path = []
target_path = []
try:
# run ctrl.generate once to load all functions
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(q=zeros, dq=zeros, target_pos=target_xyz)
robot_config.R('EE', q=zeros)
print('\nSimulation starting...\n')
print('\nClick to move the target.')
print('\nPress space to turn on adaptation.\n\n')
count = 0
