start = robot_config.Tx("EE", q=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)
interface.set_xyz(name="obstacle", xyz=obstacle_xyz)
count = 0.0
obs_count = 0.0
print("\nSimulation starting...\n")
while count < 1500:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack(
[interface.get_xyz("target"),
interface.get_orientation("target")])
# calculate the control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# get obstacle position from CoppeliaSim
obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632
# update avoidance system about obstacle position
avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]])
if moving_obstacle is True:
obs_x = 0.125 + 0.25 * np.sin(obs_count)
ctrlr = Joint(robot_config, kp=50)
# create interface and connect
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()
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
try:
count = 0
print("\nSimulation starting...\n")
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
target = np.hstack(
[interface.get_xyz("target"), interface.get_orientation("target")]
)
u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target,)
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
ee_track.append(np.copy(hand_xyz))
ee_angles_track.append(
transformations.euler_from_matrix(
robot_config.R("EE", feedback["q"]), axes="rxyz"
)
)
tmp_target = np.copy(hand_xyz[:3])
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)
count = 0.0
while 1:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack(
[interface.get_xyz("target"),
interface.get_orientation("target")])
# 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[1:3] = adapt.generate(
input_signal=np.array([feedback["q"][1], feedback["q"][2]]),
training_signal=np.array(
[ctrlr.training_signal[1], ctrlr.training_signal[2]]),
)
start = robot_config.Tx("EE", q=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)
interface.set_xyz(name="obstacle", xyz=obstacle_xyz)
count = 0.0
obs_count = 0.0
print("\nSimulation starting...\n")
while count < 1500:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack(
[interface.get_xyz("target"),
interface.get_orientation("target")])
# calculate the control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# get obstacle position from CoppeliaSim
obs_x, obs_y, obs_z = interface.get_xyz("obstacle")
# update avoidance system about obstacle position
avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]])
if moving_obstacle is True:
obs_x = 0.125 + 0.25 * np.sin(obs_count)