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')
