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()
starting_orientation = robot_config.quaternion("EE", feedback["q"])
mag = 0.6
target_position = np.random.random(3) * 0.5
target_position = target_position / np.linalg.norm(target_position) * mag
position_planner.generate_path(position=hand_xyz,
target_position=target_position)
target_orientation = transformations.random_quaternion()
orientation_path.match_position_path(
orientation=starting_orientation,
target_orientation=target_orientation,
position_path=position_planner.position_path,
)
return position_planner, orientation_path, target_position, target_orientation
# 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,
)
target_xyz = np.array([0.3, 0.3, 0.5])
target_orientation = transformations.random_quaternion()
# set up lists for tracking data
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"])
)
# set up lists for tracking data
q_track = []
target_track = []
error_track = []
target_geom_id = interface.sim.model.geom_name2id("target")
green = [0, 0.9, 0, 0.5]
red = [0.9, 0, 0, 0.5]
threshold = 0.002 # threshold distance for being within target before moving on
# get the end-effector's initial position
np.random.seed(0)
target_quaternions = [
transformations.unit_vector(transformations.random_quaternion())
for ii in range(4)
]
target_index = 0
target = target_quaternions[target_index]
print(target)
target_xyz = robot_config.Tx("EE", q=target)
interface.set_mocap_xyz(name="target", xyz=target_xyz)
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx("EE", feedback["q"])
count = 0.0
null_controllers=[damping],
# control (alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[False, False, False, True, True, True],
)
# set up lists for tracking data
ee_angles_track = []
target_angles_track = []
try:
print("\nSimulation starting...\n")
cnt = 0
while 1:
if cnt % 500 == 0:
# generate a random target orientation
rand_orient = transformations.random_quaternion()
print("New target orientation: ", rand_orient)
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
# set our target to our ee_xyz since we are only focussing on orinetation
interface.set_mocap_xyz("target_orientation", hand_xyz)
interface.set_mocap_orientation("target_orientation", rand_orient)
target = np.hstack([