def on_click(self, mouse_x, mouse_y): self.target[0] = self.mouse_x self.target[1] = self.mouse_y def on_keypress(self, key): if key == pygame.K_SPACE: 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']) target_angles = np.zeros(3) interface.set_target(target_xyz) # get Jacobians to each link for calculating perturbation J_links = [ robot_config._calc_J('link%s' % ii, x=[0, 0, 0]) for ii in range(robot_config.N_LINKS) ]
# initialize our robot config robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) # create an operational space controller ctrlr = Sliding(robot_config) def on_click(self, mouse_x, mouse_y): self.target[0] = self.mouse_x self.target[1] = self.mouse_y # create our interface interface = PyGame(robot_config, arm_sim, dt=.001, on_click=on_click) interface.connect() # create a target feedback = interface.get_feedback() target_xyz = robot_config.Tx('EE', feedback['q']) interface.set_target(target_xyz) try: print('\nSimulation starting...') print('Click to move the target.\n') count = 0 while 1: # get arm feedback feedback = interface.get_feedback()
robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) if use_force_control: # create an operational space controller ctrlr = Joint(robot_config, kp=100, kv=30) # create our path planner n_timesteps = 2000 path_planner = path_planners.InverseKinematics(robot_config) # create our interface dt = 0.001 interface = PyGame(robot_config, arm_sim, dt=dt) interface.connect() feedback = interface.get_feedback() try: print('\nSimulation starting...') print('Click to move the target.\n') count = 0 while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) if count % n_timesteps == 0: target_xyz = np.array(
# from abr_control.arms import twolink as arm from abr_control.interfaces import PyGame from abr_control.controllers import OSC, signals # initialize our robot config robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) def on_click(self, mouse_x, mouse_y): self.circles[0][0] = mouse_x 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 = []
from abr_control.interfaces import PyGame from abr_control.controllers import OSC, AvoidObstacles, Damping # initialize our robot config robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) def on_click(self, mouse_x, mouse_y): self.circles[0][0] = mouse_x self.circles[0][1] = mouse_y # create our interface interface = PyGame(robot_config, arm_sim, on_click=on_click) interface.connect() avoid = AvoidObstacles(robot_config, threshold=1) # damp the movements of the arm damping = Damping(robot_config, kv=10) # create an operational space controller ctrlr = OSC(robot_config, kp=20, vmax=10, null_controllers=[avoid, damping]) # 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)
# initialize our robot config robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) def on_click(self, mouse_x, mouse_y): self.target[0] = self.mouse_x self.target[1] = self.mouse_y # create our interface interface = PyGame(robot_config, arm_sim, dt=.001, on_click=on_click, q_init=[np.pi / 4, np.pi / 2, np.pi / 2]) interface.connect() ctrlr = OSC(robot_config, kp=100, vmax=10) avoid = signals.AvoidJointLimits( robot_config, min_joint_angles=[np.pi / 5] * robot_config.N_JOINTS, max_joint_angles=[np.pi / 2] * robot_config.N_JOINTS, max_torque=[100] * robot_config.N_JOINTS) # create a target target_xyz = [0, 2, 0] interface.set_target(target_xyz)
# create our arm simulation arm_sim = arm.ArmSim(robot_config) # create an operational space controller ctrlr = OSC(robot_config, kp=200, vmax=10) # create our path planner n_timesteps = 250 path_planner = path_planners.SecondOrder(robot_config, n_timesteps=n_timesteps, w=1e4, zeta=2) dt = 0.001 # create our interface interface = PyGame(robot_config, arm_sim, dt=dt) interface.connect() # set up lists for tracking data ee_path = [] target_path = [] pregenerate_path = False print('\nPregenerating path to follow: ', pregenerate_path, '\n') try: # run ctrl.generate once to load all functions zeros = np.zeros(robot_config.N_JOINTS) ctrlr.generate(q=zeros, dq=zeros, target_pos=zeros, target_vel=zeros) robot_config.orientation('EE', q=zeros) print('\nSimulation starting...\n')