def test_timing(arm, config_params, osc_params, use_cython):
robot_config = arm.Config(use_cython=use_cython, **config_params)
ctrlr = OSC(robot_config, **osc_params)
# then run for timing
n_trials = 1000
times = np.zeros(n_trials + 1)
for ii in range(n_trials + 1):
q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi
dq = np.random.random(robot_config.N_JOINTS) * 5
target = np.random.random(6) * 2 - 1
start = timeit.default_timer()
ctrlr.generate(q=q, dq=dq, target=target)
times[ii] = timeit.default_timer() - start
# strip off first loop to remove load time
times = times[1:]
average_time = np.sum(times) / n_trials
return average_time
def test_calc_orientation_forces(arm, orientation_algorithm):
robot_config = arm.Config(use_cython=False)
ctrlr = OSC(robot_config, orientation_algorithm=orientation_algorithm)
for ii in range(100):
q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi
quat = robot_config.quaternion("EE", q=q)
theta = np.pi / 2
axis = np.array([0, 0, 1])
quat_rot = transformations.unit_vector(
np.hstack([np.cos(theta / 2),
np.sin(theta / 2) * axis]))
quat_target = transformations.quaternion_multiply(quat, quat_rot)
target_abg = transformations.euler_from_quaternion(quat_target,
axes="rxyz")
# calculate current position quaternion
R = robot_config.R("EE", q=q)
quat_1 = transformations.unit_vector(
transformations.quaternion_from_matrix(R))
dist1 = calc_distance(quat_1, np.copy(quat_target))
# calculate current position quaternion with u_task added
u_task = ctrlr._calc_orientation_forces(target_abg, q=q)
dq = np.dot(np.linalg.pinv(robot_config.J("EE", q)),
np.hstack([np.zeros(3), u_task]))
q_2 = q - dq * 0.001 # where 0.001 represents the time step
R_2 = robot_config.R("EE", q=q_2)
quat_2 = transformations.unit_vector(
transformations.quaternion_from_matrix(R_2))
dist2 = calc_distance(quat_2, np.copy(quat_target))
assert np.abs(dist2) < np.abs(dist1)
def test_velocity_limiting(arm, ctrlr_dof):
# Derivation worked through at studywolf.wordpress.com/2016/11/07/ +
# velocity-limiting-in-operational-space-control/
robot_config = arm.Config()
kp = 10
ko = 8
kv = 4
vmax = 1
ctrlr = OSC(robot_config,
kp=kp,
ko=ko,
kv=kv,
ctrlr_dof=ctrlr_dof,
vmax=[vmax, vmax])
answer = np.zeros(6)
# xyz < vmax, abg < vmax
u_task_unscaled = np.array([0.05, 0.05, 0.05, 0.05, 0.05, 0.05])
answer[:3] = kp * u_task_unscaled[:3]
answer[3:] = ko * u_task_unscaled[3:]
output = ctrlr._velocity_limiting(u_task_unscaled)
assert np.allclose(output, answer, atol=1e-5)
# xyz > vmax, abg < vmax
u_task_unscaled = np.array([100.0, 100.0, 100.0, 0.05, 0.05, 0.05])
answer[:3] = kv * np.sqrt(vmax / 3.0)
answer[3:] = ko * u_task_unscaled[3:]
output = ctrlr._velocity_limiting(u_task_unscaled)
assert np.allclose(output, answer, atol=1e-5)
# xyz < vmax, abg > vmax
u_task_unscaled = np.array([0.05, 0.05, 0.05, 100.0, 100.0, 100.0])
answer[:3] = kp * u_task_unscaled[:3]
answer[3:] = kv * np.sqrt(vmax / 3.0)
output = ctrlr._velocity_limiting(u_task_unscaled)
assert np.allclose(output, answer, atol=1e-5)
# xyz > vmax, abg > vmax
u_task_unscaled = np.array([100.0, 100.0, 100.0, 100.0, 100.0, 100.0])
answer[:3] = kv * np.sqrt(vmax / 3.0)
answer[3:] = kv * np.sqrt(vmax / 3.0)
output = ctrlr._velocity_limiting(u_task_unscaled)
assert np.allclose(output, answer, atol=1e-5)
# initialize our robot config for the jaco2
robot_config = arm(arm_model)
# create our interface
interface = Mujoco(robot_config, dt=0.001)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# 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"])
def get_target(robot_config):
# pregenerate our path and orientation planners
n_timesteps = 2000
position_planner = path_planners.SecondOrderDMP(error_scale=0.01,
n_timesteps=n_timesteps)
orientation_path = path_planners.Orientation()
# 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 = []
try:
# run ctrl.generate once to load all functions
use_wall_clock = True
if len(sys.argv) > 1:
show_plot = sys.argv[1]
else:
show_plot = False
model_filename = 'threejoint'
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
ctrlr = OSC(robot_config,
kp=10,
kv=5,
ctrlr_dof=[True, True, True, False, False, False])
interface.send_target_angles(np.ones(3))
target_xyz = np.array([0.1, 0.1, 0.3])
interface.set_mocap_xyz('target', target_xyz)
interface.set_mocap_xyz('hand', np.array([.2, .4, 1]))
# 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)
model_filename = "twojoint"
ctrlr_dof = [True, True, False, False, False, False]
elif N_JOINTS == 3:
model_filename = "threejoint"
ctrlr_dof = [True, True, True, False, False, False]
else:
raise Exception("Only 1-3 joint arms are available in this example")
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
ctrlr = OSC(robot_config, kp=10, kv=5, ctrlr_dof=ctrlr_dof)
interface.send_target_angles(np.ones(N_JOINTS))
target = np.array([0.1, 0.1, 0.3, 0, 0, 0])
interface.set_mocap_xyz("target", target[:3])
interface.set_mocap_xyz("hand", np.array([0.2, 0.4, 1]))
q_track = []
count = 0
link_name = "EE"
try:
while True:
from abr_control.controllers import OSC, AvoidObstacles, Damping
# initialize our robot config
robot_config = arm.Config()
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)
avoid = AvoidObstacles(robot_config, threshold=1, gain=30)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create an operational space controller
ctrlr = OSC(
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
from abr_control.arms import threelink as arm
# from abr_control.arms import twolink as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC, path_planners
print('\nClick to move the target.\n')
# 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 = OSC(robot_config, kp=100, vmax=10)
# create our path planner
n_timesteps = 250 # give .25s to reach target
path_planner = path_planners.Linear(robot_config)
# create our interface
interface = PyGame(robot_config, arm_sim, dt=.001)
interface.connect()
# set up lists for tracking data
ee_path = []
target_path = []
try:
count = 0
from abr_control.arms import ur5 as arm
from abr_control.controllers import OSC, Damping, path_planners
from abr_control.interfaces import VREP
from abr_control.utils import transformations
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=100, # position gain
ko=250, # 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])
# create our interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# pregenerate our path and orientation planners
n_timesteps = 1000
traj_planner = path_planners.BellShaped(error_scale=50,
n_timesteps=n_timesteps)
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
of the end-effector is plotted in 3D.
"""
import numpy as np
from abr_control.arms import ur5 as arm
# from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC, signals
from abr_control.interfaces import VREP
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# if using the Jaco 2 arm with the hand attached, use the following instead:
# robot_config = arm.Config(use_cython=True, hand_attached=False)
# instantiate the REACH controller with obstacle avoidance
ctrlr = OSC(robot_config, kp=200, vmax=0.5)
avoid = signals.AvoidObstacles(robot_config)
# create our VREP interface
interface = VREP(robot_config, dt=.001)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
obstacle_track = []
try:
num_targets = 0
back_to_start = False
import numpy as np
from abr_control.arms import threelink as arm
# from abr_control.arms import twolink as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC
print('\nClick to move the target.\n')
# 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 = OSC(robot_config, kp=300, vmax=100, use_dJ=False, use_C=True)
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)
import numpy as np
from abr_control.arms import ur5 as arm
# from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC, AvoidObstacles, Damping
from abr_control.interfaces import VREP
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# if using the Jaco 2 arm with the hand attached, use the following instead:
# robot_config = arm.Config(use_cython=True, hand_attached=False)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# instantiate the REACH controller with obstacle avoidance
ctrlr = OSC(robot_config, kp=200, vmax=0.5, null_controllers=[damping])
avoid = AvoidObstacles(robot_config)
# create our VREP interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
obstacle_track = []
try:
num_targets = 0
back_to_start = False
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)
avoid = AvoidJointLimits(
robot_config,
min_joint_angles=[np.pi / 5.0] * robot_config.N_JOINTS,
max_joint_angles=[np.pi / 2.0] * robot_config.N_JOINTS,
max_torque=[100.0] * robot_config.N_JOINTS)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create an operational space controller
ctrlr = OSC(
robot_config,
kp=100,
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=.001,
on_click=on_click,
q_init=[np.pi / 4, np.pi / 2, np.pi / 2])
import abr_jaco2
plot = True
ctrlr_dof = [False, False, True, True, True, True]
# initialize our robot config
robot_config = abr_jaco2.Config()
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=50,
ko=100,
kv=20,
null_controllers=[damping],
vmax=None, #vmax=[10, 10], # [m/s, rad/s]
# control (x, y, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=ctrlr_dof)
# create our interface
interface = abr_jaco2.Interface(robot_config)
interface.connect()
interface.init_position_mode()
interface.send_target_angles(robot_config.START_ANGLES)
# pregenerate our path and orientation planners
n_timesteps = 10000
# traj_planner = path_planners.BellShaped(
# error_scale=1, n_timesteps=n_timesteps)
print(stars)
dt = 0.001
# create our interface
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# 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")
"""
import numpy as np
from abr_control.arms import threejoint as arm
# from abr_control.arms import twojoint as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC, path_planners
# initialize our robot config for the ur5
robot_config = arm.Config(use_cython=True)
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)
# create an operational space controller
ctrlr = OSC(robot_config, kp=200, vmax=None)
# 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 = []
import numpy as np
from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC
from abr_control.interfaces import VREP
# initialize our robot config for the ur5
robot_config = arm.Config(use_cython=True, hand_attached=True)
# instantiate controller
ctrlr = OSC(robot_config, kp=200, vmax=0.5)
# create our VREP interface
interface = VREP(robot_config, dt=.0005)
interface.connect()
target_xyz = np.array([0.5, 0.2, 0.5])
# set the target object's position in VREP
interface.set_xyz(name='target', xyz=target_xyz)
count = 0.0
while count < 1500: # run for 1.5 simulated seconds
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback['q'],
dq=feedback['dq'],
target_pos=target_xyz)
# send forces into VREP, step the sim forward
interface.send_forces(u)
count += 1
interface.disconnect()
if len(sys.argv) > 1:
show_plot = sys.argv[1]
else:
show_plot = False
model_filename = "threejoint"
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
ctrlr = OSC(
robot_config, kp=30, kv=20, ctrlr_dof=[True, True, True, False, False, False]
)
interface.send_target_angles(np.ones(3))
target_xyz = np.array([0.1, 0.1, 0.3])
interface.set_mocap_xyz("target", target_xyz)
interface.set_mocap_xyz("hand", np.array([0.2, 0.4, 1]))
target_geom_id = interface.sim.model.geom_name2id("target")
green = [0, 0.9, 0, 0.5]
red = [0.9, 0, 0, 0.5]
# create our path planner
params = {}
import numpy as np
from abr_control.arms import threejoint as arm
# from abr_control.arms import twojoint as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC, Damping, path_planners
# initialize our robot config for the ur5
robot_config = arm.Config(use_cython=True)
# create our arm simulation
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=200, null_controllers=[damping])
# create our path planner
n_timesteps = 250
path_planner = path_planners.SecondOrder(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 = []
# last for 1 second of real-time
use_wall_clock = True
# initialize our robot config
robot_config = arm.Config()
# create our arm simulation
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=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
from abr_control.controllers import OSC, Damping
from abr_control.interfaces import VREP
from abr_control.utils import transformations
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# if using the Jaco 2 arm with the hand attached, use the following instead:
# robot_config = arm.Config(use_cython=True, hand_attached=False)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=200, # position gain
ko=200, # orientation gain
null_controllers=[damping],
# control (alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[False, False, False, True, True, True])
# create our interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# set up lists for tracking data
ee_angles_track = []
target_angles_track = []
try:
print('\nSimulation starting...\n')
while 1:
def run_test(self,
n_neurons=1000,
n_ensembles=1,
decimal_scale=1,
test_name="adaptive_training",
session=None,
run=None,
weights_file=None,
pes_learning_rate=1e-6,
backend=None,
autoload=False,
time_limit=30,
target_type='single',
offset=None,
avoid_limits=False,
additional_mass=0,
kp=20,
kv=6,
ki=0,
integrate_err=False,
ee_adaptation=False,
joint_adaptation=False,
simulate_wear=False,
probe_weights=False,
seed=None,
friction_bootstrap=False,
redis_adaptation=False,
SCALES=None,
MEANS=None):
"""
The test script used to collect data for training. The use of the
adaptive controller and the type of backend can be specified. The script is
also automated to use the correct weights files for continuous learning.
The standard naming convention used is runs are consecutive tests where the previous
learned weights are used. The whole of these runs are a single session.
Multiple sessions of these runs can then be used for averaging and
statictical purposes.
The adaptive controller uses a 6 dimensional input and returns a 3 dimensional
output. The base, shoulder and elbow joints' positions and velocities are
used as the input, and the control signal for the corresponding joints gets
returned.
Parameters
----------
n_neurons: int, Optional (Default: 1000)
the number of neurons in the adaptive population
n_ensembles: int, Optional (Default: 1)
the number of ensembles of n_neurons number of neurons
decimal_scale: int, Optional (Default: 1)
used for scaling the spinnaker input. Due to precision limit, spinnaker
input gets scaled to allow for more decimals, then scaled back once
extracted. Also can be used to account for learning speed since
spinnaker runs in real time
test_name: string, Optional (Default: "dewolf_2017_data")
folder name where data is saved
session: int, Optional (Default: None)
The current session number, if left to None then it will be automatically
updated based on the current session. If the next session is desired then
this will need to be updated.
run: int, Optional (Default: None)
The current nth run that specifies to use the weights from the n-1 run.
If set to None if will automatically increment based off the last saved
weights.
weights_file: string, Optional (Default: None)
the path to the desired saved weights to use. If None will
automatically take the most recent weights saved in the 'test_name'
directory
pes_learning_rate: float, Optional (Default: 1e-6)
the learning rate for the adaptive population
backend: string
None: non adaptive control, Optional (Default: None)
'nengo': use nengo as the backend for the adaptive population
'spinnaker': use spinnaker as the adaptive population
autoload: boolean, Optional (Default: False)
True: used the specified weights, or the last set of learned weights if
not specified
False: use the specified weights, if not specified start learning from
zero
time_limit: float, Optional (Default: 30)
the time limit for each run in seconds
target_type: string, Optional (Default: single)
single: one target
multi: five targets
figure8: move in figure8 pattern
vision: get target from vision system
offset: float array, Optional (Default: None)
Set the offset to the end effector if something other than the default
is desired. Use the form [x_offset, y_offset, z_offset]
avoid_limits: boolean, Optional (Default: False)
set true if there are limits you would like to avoid
additional_mass: float, Optional (Default: 0)
any extra mass added to the EE if known [kg]
kp: float, Optional (Default: 20)
proportional gain term
kv: float, Optional (Default: 6)
derivative gain term
ki: float, Optional (Default: 0)
integral gain term
integrate_err: Boolean, Optional (Default: False)
True to add I term to PD control (PD -> PID)
ee_adaptation: Booleanm Optional (Default: False)
True if doing neural adaptation in task space
joint_adaptation: Boolean Optional (Default: False)
True if doing neural adaptation in joint space
simulate_wear: Boolean Optional (Default: False)
True to add a simulated wearing of the joints that mimics friction
by opposing motion
probe_weights: Boolean Optional (Default: False)
True to probe adaptive population for decoders
seed: int Optional, (Default: None)
seed used for random number generation in adaptive population
friction_bootstrap: Boolean Optional (Default: False)
True if want to pass an estimate of the friction signal as starting
point for adaptive population
redis_adaptation: Boolean Optional (Default: False)
True to send adaptive inputs to redis and read outputs back, allows
user to use any backend they like as long as it can read/write from
redis
Attributes
----------
TARGET_XYZ: array of floats
the goal target, used for calculating error
target: array of floats [x, y, z, vx, vy, vz]
the filtered target, used for calculating error for figure8
test since following trajectory and not moving to a single final
point
"""
print("RUN PASSED IN IS: ", run)
if redis_adaptation:
try:
import redis
redis_server = redis.StrictRedis(host='localhost')
except ImportError:
print(
"You must install redis to do adaptation through a redis" +
" server")
# Set the target based on the source and type of test
PRESET_TARGET = np.array([[.57, 0.03, .87]])
if target_type == 'multi':
PRESET_TARGET = np.array([[.56, -.09, .95], [.12, .15, .80],
[.80, .26, .61], [.38, .46, .81],
[.0, .0, 1.15]])
time_limit /= len(PRESET_TARGET)
elif target_type == 'vision':
# try to setup redis server if vision targets are desired
try:
import redis
redis_server = redis.StrictRedis(host='localhost')
self.redis_server = redis_server
except ImportError:
print(
'ERROR: Install redis to use vision targets, using preset targets'
)
elif target_type == 'figure8':
try:
import pydmps
except ImportError:
print('\npydmps library required, see ' +
'https://github.com/studywolf/pydmps\n')
# create a dmp that traces a figure8
x = np.linspace(0, np.pi * 2, 100)
dmps_traj = np.array([0.2 * np.sin(x), 0.2 * np.sin(2 * x) + 0.7])
dmps = pydmps.DMPs_rhythmic(n_dmps=2, n_bfs=50, dt=0.001)
dmps.imitate_path(dmps_traj)
# get averages and scale of target locations for scaling into network
x_avg = np.mean(PRESET_TARGET[:, 0])
y_avg = np.mean(PRESET_TARGET[:, 1])
z_avg = np.mean(PRESET_TARGET[:, 2])
x_scale = np.max(PRESET_TARGET[:, 0]) - np.min(PRESET_TARGET[:, 0])
y_scale = np.max(PRESET_TARGET[:, 1]) - np.min(PRESET_TARGET[:, 1])
z_scale = np.max(PRESET_TARGET[:, 2]) - np.min(PRESET_TARGET[:, 2])
# initialize our robot config
robot_config = abr_jaco2.Config(use_cython=True,
hand_attached=True,
SCALES=SCALES,
MEANS=MEANS)
self.robot_config = robot_config
zeros = np.zeros(robot_config.N_JOINTS)
# get Jacobians to each link for calculating perturbation
self.J_links = [
robot_config._calc_J('link%s' % ii, x=[0, 0, 0])
for ii in range(robot_config.N_LINKS)
]
self.JEE = robot_config._calc_J('EE', x=[0, 0, 0])
# account for wrist to fingers offset
R_func = robot_config._calc_R('EE')
# Use user defined offset if one is specified
if offset is None:
OFFSET = robot_config.OFFSET
else:
OFFSET = offset
robot_config.Tx('EE', q=zeros, x=OFFSET)
# temporarily set backend to nengo if non adaptive if selected so we can
# still use the weight saving function in dynamics_adaptation.py in the
# abr_control repo
if backend is None:
adapt_backend = 'nengo'
else:
adapt_backend = backend
# create our adaptive controller
if ee_adaptation and joint_adaptation:
n_input = 4
n_output = 3
elif ee_adaptation and not joint_adaptation:
n_input = 3
n_output = 3
pes_learning_rate /= ki
elif joint_adaptation and not ee_adaptation:
n_input = 4
n_output = 2
else:
n_input = 1
n_output = 1
# for use with weight estimation, is the number of joint angles being
# passed to the adaptive controller (not including velocities)
self.adapt_dim = n_output
self.decimal_scale = decimal_scale
# if user specifies an additional mass, use the estimation function to
# give the adaptive controller a better starting point
self.additional_mass = additional_mass
if self.additional_mass != 0 and weights_file is None:
# if the user knows about the mass at the EE, try and improve
# our starting point
self.fake_gravity = np.array(
[[0, 0, -9.81 * self.additional_mass, 0, 0, 0]]).T
print('Using mass estimate of %f kg as starting point' %
self.additional_mass)
adapt = signals.DynamicsAdaptation(
n_input=n_input,
n_output=n_output,
n_neurons=n_neurons,
n_ensembles=n_ensembles,
pes_learning_rate=pes_learning_rate,
intercepts=(-0.5, -0.2),
weights_file=weights_file,
backend=adapt_backend,
session=session,
run=run,
test_name=test_name,
autoload=autoload,
function=self.gravity_estimate,
probe_weights=probe_weights,
seed=seed)
elif friction_bootstrap and weights_file is None:
# if the user knows about the friction, try and improve
# our starting point
print('Using friction estimate as starting point')
adapt = signals.DynamicsAdaptation(
n_input=n_input,
n_output=n_output,
n_neurons=n_neurons,
n_ensembles=n_ensembles,
pes_learning_rate=pes_learning_rate,
intercepts=(-0.5, -0.2),
weights_file=weights_file,
backend=adapt_backend,
session=session,
run=run,
test_name=test_name,
autoload=autoload,
function=lambda x: self.friction(x) * -1,
probe_weights=probe_weights,
seed=seed)
else:
adapt = signals.DynamicsAdaptation(
n_input=n_input,
n_output=n_output,
n_neurons=n_neurons,
n_ensembles=n_ensembles,
pes_learning_rate=pes_learning_rate,
intercepts=(-0.5, -0.2),
weights_file=weights_file,
backend=adapt_backend,
session=session,
run=run,
test_name=test_name,
autoload=autoload,
probe_weights=probe_weights,
seed=seed)
# get save location of saved data
[location, run_num] = adapt.weights_location(test_name=test_name,
run=run,
session=session)
# if using integral term for PID control, check if previous weights
# have been saved
if integrate_err:
if run_num < 1:
run_num = 0
int_err_prev = [0, 0, 0]
else:
int_err_prev = np.squeeze(
np.load(location + '/run%i_data/int_err%i.npz' %
(run_num - 1, run_num - 1))['int_err'])[-1]
else:
int_err_prev = None
# instantiate operational space controller
print('using int err of: ', int_err_prev)
ctrlr = OSC(robot_config,
kp=kp,
kv=kv,
ki=ki,
vmax=1,
null_control=True,
int_err=int_err_prev)
# add joint avoidance controller if specified to do so
if avoid_limits:
avoid = signals.AvoidJointLimits(
robot_config,
# joint 4 flipped because does not cross 0-2pi line
min_joint_angles=[None, 1.1, 0.5, 3.5, 2.0, 1.6],
max_joint_angles=[None, 3.65, 6.25, 6.0, 5.0, 4.6],
max_torque=[4] * robot_config.N_JOINTS,
cross_zero=[True, False, False, False, True, False],
gradient=[False, True, True, True, True, False])
# if not planning the trajectory (figure8 target) then use a second
# order filter to smooth out the trajectory to target(s)
if target_type != 'figure8':
path = path_planners.SecondOrder(robot_config)
n_timesteps = 4000
w = 1e4 / n_timesteps
zeta = 2
dt = 0.003
# run controller once to generate functions / take care of overhead
# outside of the main loop, because force mode auto-exits after 200ms
ctrlr.generate(zeros, zeros, np.zeros(3), offset=OFFSET)
interface = abr_jaco2.Interface(robot_config)
# connect to and initialize the arm
interface.connect()
interface.init_position_mode()
interface.send_target_angles(robot_config.INIT_TORQUE_POSITION)
# set up lists for tracking data
time_track = []
q_track = []
dq_track = []
u_track = []
adapt_track = []
error_track = []
training_track = []
target_track = []
ee_track = []
input_signal = []
if integrate_err:
int_err_track = []
if simulate_wear:
self.F_prev = np.array([0, 0])
friction_track = []
try:
interface.init_force_mode()
for ii in range(0, len(PRESET_TARGET)):
# counter for print statements
count = 0
# track loop_time for stopping test
loop_time = 0
# get joint angle and velocity feedback
feedback = interface.get_feedback()
q = feedback['q']
dq = feedback['dq']
dq[abs(dq) < 0.05] = 0
# calculate end-effector position
ee_xyz = robot_config.Tx('EE', q=q, x=OFFSET)
# last three terms used as started point for target EE velocity
target = np.concatenate((ee_xyz, np.array([0, 0, 0])), axis=0)
# M A I N C O N T R O L L O O P
while loop_time < time_limit:
start = timeit.default_timer()
prev_xyz = ee_xyz
if target_type == 'vision':
TARGET_XYZ = self.get_target_from_camera()
TARGET_XYZ = self.normalize_target(TARGET_XYZ)
target = path.step(y=target[:3],
dy=target[3:],
target=TARGET_XYZ,
w=w,
zeta=zeta,
dt=dt,
threshold=0.05)
elif target_type == 'figure8':
y, dy, ddy = dmps.step()
target = [0.65, y[0], y[1], 0, dy[0], dy[1]]
TARGET_XYZ = target[:3]
else:
TARGET_XYZ = PRESET_TARGET[ii]
target = path.step(y=target[:3],
dy=target[3:],
target=TARGET_XYZ,
w=w,
zeta=zeta,
dt=dt,
threshold=0.05)
# calculate euclidean distance to target
error = np.sqrt(np.sum((ee_xyz - TARGET_XYZ)**2))
# get joint angle and velocity feedback
feedback = interface.get_feedback()
q = feedback['q']
dq = feedback['dq']
dq[abs(dq) < 0.05] = 0
# calculate end-effector position
ee_xyz = robot_config.Tx('EE', q=q, x=OFFSET)
if ee_adaptation and joint_adaptation:
# adaptive control in state space
training_signal = TARGET_XYZ - ee_xyz
# calculate the adaptive control signal
adapt_input = np.array([
robot_config.scaledown('q', q)[1],
robot_config.scaledown('q', q)[2],
robot_config.scaledown('dq', dq)[1],
robot_config.scaledown('dq', dq)[2]
])
u_adapt = adapt.generate(
input_signal=adapt_input,
training_signal=training_signal)
u_adapt /= decimal_scale
elif ee_adaptation and not joint_adaptation:
# adaptive control in state space
training_signal = TARGET_XYZ - ee_xyz
# scale inputs
adapt_input = np.array([(ee_xyz[0] - x_avg) / x_scale,
(ee_xyz[1] - y_avg) / y_scale,
(ee_xyz[2] - z_avg) / z_scale])
u_adapt = adapt.generate(
input_signal=adapt_input,
training_signal=training_signal)
u_adapt /= decimal_scale
else:
u_adapt = None
# Need to create controller here, in case using ee
# adaptation, it needs to be passed to the OSC controller
# calculate the base operation space control signal
u_base = ctrlr.generate(q=q,
dq=dq,
target_pos=target[:3],
target_vel=target[3:],
offset=OFFSET,
ee_adapt=u_adapt)
# account for stiction in jaco2 base
if u_base[0] > 0:
u_base[0] *= 3.0
else:
u_base[0] *= 2.0
if joint_adaptation and not ee_adaptation:
training_signal = np.array([
ctrlr.training_signal[1], ctrlr.training_signal[2]
])
# calculate the adaptive control signal
adapt_input = np.array([
robot_config.scaledown('q', q)[1],
robot_config.scaledown('q', q)[2],
robot_config.scaledown('dq', dq)[1],
robot_config.scaledown('dq', dq)[2]
])
u_adapt = adapt.generate(
input_signal=adapt_input,
training_signal=training_signal)
# add adaptive signal to base controller
u_adapt = np.array([
0, u_adapt[0] / decimal_scale,
u_adapt[1] / decimal_scale, 0, 0, 0
])
u = u_base + u_adapt
elif redis_adaptation:
training_signal = np.array([
ctrlr.training_signal[1], ctrlr.training_signal[2]
])
adapt_input = np.array([
robot_config.scaledown('q', q)[1],
robot_config.scaledown('q', q)[2],
robot_config.scaledown('dq', dq)[1],
robot_config.scaledown('dq', dq)[2]
])
# send input information to redis
redis_server.set(
'input_signal', '%.3f %.3f %.3f %.3f' %
(adapt_input[0], adapt_input[1], adapt_input[2],
adapt_input[3]))
redis_server.set(
'training_signal', '%.3f %.3f' %
(training_signal[0], training_signal[1]))
# get adaptive output back
u_adapt = redis_server.get('u_adapt').decode('ascii')
u_adapt = np.array(
[float(val) for val in u_adapt.split()])
u_adapt = np.array(
[0, u_adapt[0], u_adapt[1], 0, 0, 0])
u = u_base + u_adapt
else:
u = u_base
u_adapt = None
training_signal = np.zeros(3)
adapt_input = np.zeros(3)
# add limit avoidance if True
if avoid_limits:
u += avoid.generate(q)
if simulate_wear:
u_friction = self.friction(dq[1:3])
u += [0, u_friction[0], u_friction[1], 0, 0, 0]
friction_track.append(np.copy(u_friction))
# send forces
interface.send_forces(np.array(u, dtype='float32'))
# track data
q_track.append(np.copy(q))
dq_track.append(np.copy(dq))
u_track.append(np.copy(u))
adapt_track.append(np.copy(u_adapt))
error_track.append(np.copy(error))
training_track.append(np.copy(training_signal))
end = timeit.default_timer() - start
loop_time += end
time_track.append(np.copy(end))
target_track.append(np.copy(TARGET_XYZ))
input_signal.append(np.copy(adapt_input))
ee_track.append(np.copy(ee_xyz))
if integrate_err:
int_err_track.append(np.copy(ctrlr.int_err))
if count % 1000 == 0:
print('error: ', error)
print('dt: ', end)
#print('adapt: ', u_adapt)
#print('int_err/ki: ', ctrlr.int_err)
#print('q: ', q)
#print('hand: ', ee_xyz)
#print('target: ', target)
#print('control: ', u_base)
count += 1
except:
print(traceback.format_exc())
finally:
# close the connection to the arm
interface.init_position_mode()
print("RUN IN IS: ", run)
# get save location of weights to save tracked data in same directory
[location, run_num] = adapt.weights_location(test_name=test_name,
run=run,
session=session)
print("RUN OUT IS: ", run_num)
if backend != None:
run_num += 1
# Save the learned weights
adapt.save_weights(test_name=test_name,
session=session,
run=run)
interface.send_target_angles(robot_config.INIT_TORQUE_POSITION)
interface.disconnect()
print('**** RUN STATS ****')
print('Average loop speed: ', sum(time_track) / len(time_track))
print('AVG Error/Step: ', sum(error_track) / len(error_track))
print('Run number ', run_num)
print('Saving tracked data to ',
location + '/run%i_data' % (run_num))
print('*******************')
time_track = np.array(time_track)
q_track = np.array(q_track)
u_track = np.array(u_track)
adapt_track = np.array(adapt_track)
error_track = np.array(error_track)
training_track = np.array(training_track)
input_signal = np.array(input_signal)
ee_track = np.array(ee_track)
dq_track = np.array(dq_track)
if integrate_err:
int_err_track = np.array(int_err_track)
if simulate_wear:
friction_track = np.array(friction_track)
if not os.path.exists(location + '/run%i_data' % (run_num)):
os.makedirs(location + '/run%i_data' % (run_num))
np.savez_compressed(location + '/run%i_data/q%i' %
(run_num, run_num),
q=[q_track])
np.savez_compressed(location + '/run%i_data/time%i' %
(run_num, run_num),
time=[time_track])
np.savez_compressed(location + '/run%i_data/u%i' %
(run_num, run_num),
u=[u_track])
np.savez_compressed(location + '/run%i_data/adapt%i' %
(run_num, run_num),
adapt=[adapt_track])
np.savez_compressed(location + '/run%i_data/target%i' %
(run_num, run_num),
target=[target_track])
np.savez_compressed(location + '/run%i_data/error%i' %
(run_num, run_num),
error=[error_track])
np.savez_compressed(location + '/run%i_data/training%i' %
(run_num, run_num),
training=[training_track])
np.savez_compressed(location + '/run%i_data/input_signal%i' %
(run_num, run_num),
input_signal=[input_signal])
np.savez_compressed(location + '/run%i_data/ee_xyz%i' %
(run_num, run_num),
ee_xyz=[ee_track])
np.savez_compressed(location + '/run%i_data/dq%i' %
(run_num, run_num),
dq=[dq_track])
if probe_weights:
np.savez_compressed(
location + '/run%i_data/probe%i' % (run_num, run_num),
probe=[adapt.sim.data[adapt.nengo_model.weights_probe]])
if integrate_err:
np.savez_compressed(location + '/run%i_data/int_err%i' %
(run_num, run_num),
int_err=[int_err_track])
if simulate_wear:
np.savez_compressed(location + '/run%i_data/friction%i' %
(run_num, run_num),
friction=[friction_track])
if backend != 'nengo_spinnaker':
# give user time to pause before the next run starts, only
# works if looping through tests using a bash script
import time
print('2 Seconds to pause: Hit ctrl z to pause, fg to resume')
time.sleep(1)
print('1 Second to pause')
time.sleep(1)
print('Starting next test...')
clicking on the background.
"""
import numpy as np
# from abr_control.arms import threejoint as arm
from abr_control.arms import twojoint as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC
# 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 = OSC(robot_config, kp=20, vmax=None, use_C=True, use_g=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=.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)
# if set to True, the simulation will plan the path to
# last for 1 second of real-time
use_wall_clock = False
# initialize our robot config for the ur5
robot_config = arm.Config(use_cython=True)
# create our arm simulation
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=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
params = {}
if use_wall_clock:
run_time = 1 # wall clock time to run each trajectory for
time_elapsed = np.copy(run_time)
count = 0
else:
params['error_scale'] = 50
params['n_timesteps'] = 500 # time steps each trajectory lasts
count = np.copy(params['n_timesteps'])
time_elapsed = 0.0
path_planner = path_planners.BellShaped(**params)
from abr_control.controllers import OSC, signals, path_planners
import abr_jaco2
plot_error = True
# initialize our robot config
robot_config = abr_jaco2.Config()
# instantiate our null controllers
null_controllers = []
from abr_control.controllers import Damping
null_controllers.append(Damping(
robot_config=robot_config, kv=10))
# instantiate controller
ctrlr = OSC(robot_config, kp=30, kv=20, vmax=None,
null_controllers=null_controllers)
# instantiate path planner and set parameters
path = path_planners.SecondOrderFilter(
n_timesteps=3000, w=1e4, zeta=2, threshold=0.05)
# run controller once to generate functions / take care of overhead
# outside of the main loop, because force mode auto-exits after 200ms
zeros = np.zeros(robot_config.N_JOINTS)
robot_config.Tx('EE', q=zeros)
ctrlr.generate(zeros, zeros, zeros)
interface = abr_jaco2.Interface(robot_config)
import traceback
from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC, Damping, signals
from abr_control.interfaces import VREP
# initialize our robot config for the jaco2
robot_config = arm.Config(use_cython=True, hand_attached=False)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# instantiate controller
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[damping],
vmax=[0.5, 0], # [m/s, rad/s]
# control (x, y, z) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, False, False, False])
# create our adaptive controller
adapt = signals.DynamicsAdaptation(
n_neurons=1000,
n_ensembles=5,
n_input=2, # we apply adaptation on the most heavily stressed joints
n_output=2,
pes_learning_rate=5e-5,
intercepts_bounds=[-0.6, -0.2],
intercepts_mode=-0.2,
means=[3.14, 3.14],
variances=[1.57, 1.57])
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# 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)
]
# create our arm simulation
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=50, null_controllers=[damping])
# create our nonlinear adaptation controller
adapt = signals.DynamicsAdaptation(n_input=robot_config.N_JOINTS,
n_output=robot_config.N_JOINTS,
pes_learning_rate=1e-4,
backend='nengo')
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:
# from abr_control.arms import twojoint as arm
from abr_control.interfaces.pygame import PyGame
from abr_control.controllers import OSC, Damping, signals
# initialize our robot config
robot_config = arm.Config()
# create our arm simulation
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=50,
null_controllers=[damping],
# control (x, y) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, False, False, False, False],
)
# create our nonlinear adaptive controller
adapt = signals.DynamicsAdaptation(
n_input=robot_config.N_JOINTS,
n_output=robot_config.N_JOINTS,
pes_learning_rate=1e-4,
means=[0, 0, 0],
variances=[np.pi, np.pi, np.pi],
)
def on_click(self, mouse_x, mouse_y):
import timeit
from abr_control.controllers import OSC, path_planners, signals
import abr_jaco2
# initialize our robot config
robot_config = abr_jaco2.Config(use_cython=True, hand_attached=True)
# instantiate operation space controller
null_controllers = []
from abr_control.controllers import Damping
null_controllers.append(Damping(robot_config=robot_config, kv=1))
ctrlr = OSC(robot_config,
kp=30,
kv=20,
ki=0.002,
null_controllers=null_controllers)
# run controller once to generate functions / take care of overhead
# outside of the main loop, because force mode auto-exits after 200ms
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(zeros, zeros, zeros)
# offset to move control point from palm to fingers
robot_config.Tx('EE', q=zeros)
# create our interface for the jaco2
interface = abr_jaco2.Interface(robot_config)
target_xyz = np.array([[.56, -.09, .52], [.12, .15, .75], [.60, .26, .61],
[.38, .46, .81]])
# connect to the jaco
