target=target, ) # if adaptation is on (toggled with space bar) if interface.adaptation: u += adapt.generate(input_signal=feedback["q"], training_signal=ctrlr.training_signal) 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() new_target = interface.get_mousexy() if new_target is not None: target_xyz[:2] = new_target interface.set_target(target_xyz) # apply the control signal, step the sim forward interface.send_forces(u, update_display=True if count % 20 == 0 else False) count += 1 finally: # stop and reset the simulation interface.disconnect()
try: print("\nSimulation starting...") print("Click to move the obstacle.\n") count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) target = np.hstack([target_xyz, target_angles]) # generate an operational space control signal u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target,) # add in obstacle avoidance obs_xy = interface.get_mousexy() if obs_xy is not None: avoid.set_obstacles(obstacles=[[obs_xy[0], obs_xy[1], 0, 0.2]]) # apply the control signal, step the sim forward interface.send_forces(u, update_display=True if count % 20 == 0 else False) # change target location once hand is within # 5mm of the target if np.sqrt(np.sum((target_xyz - hand_xyz) ** 2)) < 0.005: target_xyz = np.array( [np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0] ) # update the position of the target interface.set_target(target_xyz)