world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
robot.print_info()
# define useful variables for FK
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
# load data
with open(os.path.dirname(os.path.abspath(__file__)) + '/data.txt', 'rb') as f:
positions = pickle.load(f)
# set initial joint position
robot.reset_joint_states(q=positions[0])
# draw a sphere at the position of the end-effector
sphere = world.load_visual_sphere(
position=robot.get_link_world_positions(link_id),
radius=0.05,
color=(1, 0, 0, 0.5))
sphere = Body(sim, body_id=sphere)
# perform simulation
for t in count():
# if no more joint positions, get out of the loop
if t >= len(positions):
break
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
qIdx = robot.get_q_indices(joint_ids)
# define gains
kp = 50 # 5 if velocity control, 50 if position control
kd = 0 # 2*np.sqrt(kp)
# create sphere to follow
sphere = world.load_visual_sphere(position=np.array([0.5, 0., 1.]),
radius=0.05,
color=(1, 0, 0, 0.5))
sphere = Body(sim, body_id=sphere)
# set initial joint positions (based on the position of the sphere at [0.5, 0, 1])
robot.reset_joint_states(q=[
8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01,
1.68308810e-05, 3.71467111e-01, 5.62890805e-05
])
# define amplitude and angular velocity when moving the sphere
w = 0.01
r = 0.2
for t in count():
# move sphere
sphere.position = np.array([
0.5, r * np.cos(w * t + np.pi / 2),
(1. - r) + r * np.sin(w * t + np.pi / 2)
])
# get current end-effector position and velocity in the task/operational space
x = robot.get_link_world_positions(link_id)