interface = CoppeliaSim(robot_config=robot_config, dt=0.005)
interface.connect()
# make the target an offset of the current configuration
feedback = interface.get_feedback()
target = feedback["q"] + np.ones(robot_config.N_JOINTS) * 0.3
# For CoppeliaSim files that have a shadow arm to show the target configuration
# get joint handles for shadow
names = ["joint%i_shadow" % ii for ii in range(robot_config.N_JOINTS)]
joint_handles = []
for name in names:
interface.get_xyz(name) # this loads in the joint handle
joint_handles.append(interface.misc_handles[name])
# move shadow to target position
interface.send_target_angles(target, joint_handles)
# set up arrays for tracking end-effector and target position
q_track = []
try:
count = 0
print("\nSimulation starting...\n")
while count < 1500:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target,)
# 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,
means=[3.14, 3.14],
variances=[1.57, 1.57],
)
# create our CoppeliaSim interface
interface = CoppeliaSim(robot_config, dt=0.005)
interface.connect()
interface.send_target_angles(q=robot_config.START_ANGLES)
# set up lists for tracking data
ee_track = []
target_track = []
# 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:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx("EE", feedback["q"])