target_velocity = next_target[3:]
interface.set_mocap_xyz("path_planner", target)
# generate an operational space control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=np.hstack((target, np.zeros(3))),
ref_frame=link_name,
)
# apply the control signal, step the sim forward
interface.send_forces(u)
ee_track.append(np.copy(hand_xyz))
target_track.append(interface.get_xyz(link_name))
count += 1
time_elapsed += timeit.default_timer() - start
error = np.linalg.norm(hand_xyz - target_xyz)
if error < 0.02:
interface.sim.model.geom_rgba[target_geom_id] = green
else:
interface.sim.model.geom_rgba[target_geom_id] = red
if count % 500 == 0:
print("error: ", error)
if use_wall_clock:
# either update target every 1s
update_target = time_elapsed >= path_planner.time_to_converge + 2
target_xyz = start + np.array([0.2, -0.2, -0.2])
interface.set_mocap_xyz(name="target", xyz=target_xyz)
# set the status of the top right text for adaptation
interface.viewer.adapt = True
count = 0.0
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack([
interface.get_xyz("target"),
transformations.euler_from_quaternion(
interface.get_orientation("target"), "rxyz"),
])
# calculate the control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
u_adapt = np.zeros(robot_config.N_JOINTS)
u_adapt[:5] = adapt.generate(
input_signal=np.hstack((feedback["q"][:5], feedback["dq"][:5])),
training_signal=np.array(ctrlr.training_signal[:5]),
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
for ii, dof in enumerate(ctrlr_dof[:3]):
if not dof:
target_xyz[ii] = hand_xyz[ii]
interface.set_mocap_xyz("target_orientation", target_xyz)
interface.set_mocap_orientation("target_orientation",
target_orientation)
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
target = np.hstack([
interface.get_xyz("target_orientation"),
transformations.euler_from_quaternion(
interface.get_orientation("target_orientation"), "rxyz"),
])
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# add gripper forces
u = np.hstack((u, np.zeros(robot_config.N_GRIPPER_JOINTS)))
# apply the control signal, step the sim forward
interface.send_forces(u)
