feedback = interface.get_feedback() start = robot_config.Tx("EE", feedback["q"]) # make the target offset from that start position target_xyz = start + np.array([0.2, -0.2, 0.0]) interface.set_xyz(name="target", xyz=target_xyz) count = 0.0 print("\nSimulation starting...\n") while count < 1500: # get joint angle and velocity feedback feedback = interface.get_feedback() # calculate the control signal u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target_xyz) # send forces into CoppeliaSim, step the sim forward interface.send_forces(u) # calculate end-effector position ee_xyz = robot_config.Tx("EE", q=feedback["q"]) # track data ee_track.append(np.copy(ee_xyz)) target_track.append(np.copy(target_xyz)) count += 1 except: print(traceback.format_exc())
# set up lists for tracking data ee_track = [] target_track = [] try: # get the end-effector's initial position feedback = interface.get_feedback() start = robot_config.Tx('EE', feedback['q']) # make the target offset from that start position target_xyz = start + np.array([0.2, -0.2, 0.0]) interface.set_xyz(name='target', xyz=target_xyz) # 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) count = 0.0 print('\nSimulation starting...\n') while count < 1500: # get joint angle and velocity feedback feedback = interface.get_feedback() # calculate the control signal u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'], target_pos=target_xyz) # send forces into VREP, step the sim forward interface.send_forces(u) # calculate end-effector position
] try: print('\nSimulation starting...') print('Click to move the target.') print('Press space to turn on adaptation.\n') count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) # generate an operational space control signal u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'], target=target_xyz) # if adaptation is on (toggled with space bar) if interface.adaptation: sig = adapt.generate(input_signal=feedback['q'], training_signal=-ctrlr.s) u += sig fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0 g = np.zeros((robot_config.N_JOINTS, 1)) for ii in range(robot_config.N_LINKS): pars = tuple(feedback['q']) + tuple([0, 0, 0]) g += np.dot(J_links[ii](*pars).T, fake_gravity) u += g.squeeze()