feedback = interface.get_feedback()
start = robot_config.Tx("EE", feedback["q"])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name="target", xyz=target_xyz)
count = 0.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_xyz)
# send forces into CoppeliaSim, step the sim forward
interface.send_forces(u)
# calculate end-effector position
ee_xyz = robot_config.Tx("EE", q=feedback["q"])
# track data
ee_track.append(np.copy(ee_xyz))
target_track.append(np.copy(target_xyz))
count += 1
except:
print(traceback.format_exc())
# set up lists for tracking data
ee_track = []
target_track = []
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx('EE', feedback['q'])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name='target', xyz=target_xyz)
# run ctrl.generate once to load all functions
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(q=zeros, dq=zeros, target_pos=target_xyz)
robot_config.R('EE', q=zeros)
count = 0.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_pos=target_xyz)
# send forces into VREP, step the sim forward
interface.send_forces(u)
# calculate end-effector position
]
try:
print('\nSimulation starting...')
print('Click to move the target.')
print('Press space to turn on adaptation.\n')
count = 0
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
# generate an operational space control signal
u = ctrlr.generate(q=feedback['q'],
dq=feedback['dq'],
target=target_xyz)
# if adaptation is on (toggled with space bar)
if interface.adaptation:
sig = adapt.generate(input_signal=feedback['q'],
training_signal=-ctrlr.s)
u += sig
fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0
g = np.zeros((robot_config.N_JOINTS, 1))
for ii in range(robot_config.N_LINKS):
pars = tuple(feedback['q']) + tuple([0, 0, 0])
g += np.dot(J_links[ii](*pars).T, fake_gravity)
u += g.squeeze()
