arm_sim = arm.ArmSim(robot_config)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create an operational space controller
ctrlr = OSC(
robot_config,
kp=500,
null_controllers=[damping],
vmax=[20, 0], # [m/s, rad/s]
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
# create our interface
interface = PyGame(robot_config, arm_sim, dt=0.001)
interface.connect()
# 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)
try:
print("\nSimulation starting...\n")
count = 1
while 1:
# get arm feedback
feedback = interface.get_feedback()
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)
]
# set the target orientation to be the initial EE
# orientation rotated by theta
R_theta = np.array(
[
[np.cos(interface.theta), -np.sin(interface.theta), 0],
[np.sin(interface.theta), np.cos(interface.theta), 0],
[0, 0, 1],
]
)
R_target = np.dot(R_theta, R)
self.target_angles = transformations.euler_from_matrix(R_target, axes="sxyz")
# create our interface
interface = PyGame(
robot_config, arm_sim, dt=0.001, on_click=on_click, on_keypress=on_keypress
)
interface.connect()
feedback = interface.get_feedback()
# set target position
target_xyz = robot_config.Tx("EE", feedback["q"])
interface.set_target(target_xyz)
# set target orientation
interface.theta = -3 * np.pi / 4
R = robot_config.R("EE", feedback["q"])
interface.on_keypress(interface, None)
try:
print("\nSimulation starting...")
print("Click to move the target.")
robot_config,
kp=10,
null_controllers=[avoid, damping],
vmax=[10, 0], # [m/s, rad/s]
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
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()
# create an obstacle
interface.add_circle(xyz=[0, 0, 0], radius=0.2)
# create a target [x, y, z]]
target_xyz = [0, 2, 0]
# create a target orientation [alpha, beta, gamma]
target_angles = [0, 0, 0]
interface.set_target(target_xyz)
try:
print("\nSimulation starting...")
print("Click to move the obstacle.\n")
robot_config = arm.Config()
# 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(
robot_config,
kp=20,
use_C=True,
null_controllers=[damping],
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
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=0.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()
null_controllers=[avoid, damping],
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
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=0.001,
on_click=on_click,
q_init=[np.pi / 4, np.pi / 2, np.pi / 2],
)
interface.connect()
# create a target [x, y, z]]
target_xyz = [0, 2, 0]
# create a target orientation [alpha, beta, gamma]
target_angles = [0, 0, 0]
interface.set_target(target_xyz)
try:
print("\nSimulation starting...\n")
count = 0
# set the target orientation to be the initial EE
# orientation rotated by theta
R_theta = np.array(
[
[np.cos(interface.theta), -np.sin(interface.theta), 0],
[np.sin(interface.theta), np.cos(interface.theta), 0],
[0, 0, 1],
]
)
R_target = np.dot(R_theta, R)
self.target_angles = transformations.euler_from_matrix(R_target, axes="sxyz")
# create our interface
interface = PyGame(robot_config, arm_sim, dt=0.001, on_keypress=on_keypress)
interface.connect()
interface.theta = -3 * np.pi / 4
feedback = interface.get_feedback()
R = robot_config.R("EE", feedback["q"])
interface.on_keypress(interface, None)
try:
print("\nSimulation starting...")
print("Press left or right arrow to change target orientation angle.\n")
count = 0
while 1:
# get arm feedback
feedback = interface.get_feedback()
robot_config,
kp=50,
ki=1e-3,
null_controllers=[damping],
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
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, on_click=on_click)
interface.connect()
# 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)
]
try:
