green = [0, 0.9, 0, 0.5]
red = [0.9, 0, 0, 0.5]
# create our path planner
params = {}
if use_wall_clock:
run_time = 4 # wall clock time to run each trajectory for
params["n_timesteps"] = 100 # time steps each trajectory lasts
time_elapsed = np.copy(run_time)
count = 0
else:
params["n_timesteps"] = 2000 # time steps each trajectory lasts
count = np.copy(params["n_timesteps"])
time_elapsed = 0.0
path_planner = PathPlanner(
pos_profile=Linear(), vel_profile=Gaussian(dt=dt, acceleration=1)
)
ee_track = []
target_track = []
count = 0
update_target = True
link_name = "EE"
try:
while True:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
start = timeit.default_timer()
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[damping],
# control (gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
# create our path planner
count = 0
if use_wall_clock:
run_time = 5 # wall clock time to run each trajectory for
time_elapsed = np.copy(run_time)
else:
time_elapsed = 0.0
path_planner = PathPlanner(pos_profile=Ellipse(horz_stretch=0.5),
vel_profile=Linear(dt=dt, acceleration=1))
# create our interface
interface = PyGame(robot_config, arm_sim, dt=dt)
interface.connect()
first_pass = True
try:
print("\nSimulation starting...")
print("Click to move the target.\n")
while 1:
start = timeit.default_timer()
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
import numpy as np
from abr_control.controllers.path_planners import PathPlanner
from abr_control.controllers.path_planners.position_profiles import SinCurve
from abr_control.controllers.path_planners.velocity_profiles import Linear
Pprof = SinCurve(axes=["x", "z"], n_sample_points=1000, cycles=[1, 1, 2])
Vprof = Linear(dt=0.001, acceleration=1)
path = PathPlanner(pos_profile=Pprof, vel_profile=Vprof, verbose=True)
path.generate_path(
start_position=np.zeros(3),
target_position=np.array([5, 3, -2]),
start_orientation=np.array([0, 0, 0]),
target_orientation=np.array([0, 0, 3.14]),
max_velocity=2,
start_velocity=0,
target_velocity=0,
plot=True,
)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30,
kv=20,
ko=180,
null_controllers=[damping],
vmax=[10, 10], # [m/s, rad/s]
# control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=ctrlr_dof,
)
path_planner = PathPlanner(
pos_profile=Linear(), vel_profile=Gaussian(dt=dt, acceleration=max_a)
)
# set up lists for tracking data
ee_track = []
target_track = []
try:
print("\nSimulation starting...\n")
for ii in range(0, n_targets):
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
pos_target = np.array(
[
np.random.uniform(low=-0.4, high=0.4),
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create an operational space controller
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[damping],
# control (gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
# create our path planner
target_dx = 0.001
path_planner = PathPlanner(
pos_profile=LinearP(), vel_profile=LinearV(dt=dt, acceleration=1)
)
# create our interface
interface = PyGame(robot_config, arm_sim, dt=dt)
interface.connect()
first_pass = True
try:
print("\nSimulation starting...")
print("Click to move the target.\n")
count = 0
dx_track = []
Example of moving between several randomly generated points where we stop at
each target (v=0)
"""
import matplotlib.pyplot as plt
import numpy as np
from abr_control.controllers.path_planners import PathPlanner
from abr_control.controllers.path_planners.position_profiles import Linear
from abr_control.controllers.path_planners.velocity_profiles import Gaussian
n_targets = 5
Pprof = Linear()
Vprof = Gaussian(dt=0.001, acceleration=1)
path_planner = PathPlanner(pos_profile=Pprof, vel_profile=Vprof)
start = np.zeros(3)
targets = np.random.uniform(low=-5, high=5, size=(n_targets, 3))
for ii, target in enumerate(targets):
if ii == 0:
start_position = start
else:
start_position = targets[ii - 1]
path = path_planner.generate_path(
start_position=start_position, target_position=target, max_velocity=2
)
if ii == 0:
position_path = path[:, :3]
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30, # position gain
kv=20,
ko=180, # orientation gain
null_controllers=[damping],
vmax=None, # [m/s, rad/s]
# control all DOF [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, True, True, True],
)
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
path_planner = PathPlanner(pos_profile=Linear(),
vel_profile=Gaussian(dt=dt, acceleration=2))
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
first_pass = True
try:
count = 0
print("\nSimulation starting...\n")
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)