# 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() print("Simulation terminated...")
count = 1 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) error = np.sqrt(np.sum((target_xyz - hand_xyz)**2)) target = np.hstack([target_xyz, target_angles]) # generate an operational space control signal u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ) # get the next point in the target trajectory from the dmp target_xyz[0], target_xyz[1] = dmps.step(error=error * 1e2)[0] interface.set_target(target_xyz) # apply the control signal, step the sim forward interface.send_forces(u, update_display=(count % 20 == 0)) count += 1 finally: # stop and reset the simulation interface.disconnect() print("Simulation terminated...")