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)
dx = robot.get_link_world_linear_velocities(link_id)
# Get joint positions
q = robot.get_joint_positions()
# Get linear jacobian
if robot.has_floating_base():
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
else:
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
# Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
Jp = robot.get_damped_least_squares_inverse(J, damping)
# evaluate damped-least-squares IK
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
# set joint positions
robot.set_joint_positions(positions[t], joint_ids=joint_ids)
# make the sphere follow the end effector
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
# set joint positions
robot.set_joint_positions(positions[t], joint_ids=joint_ids)
# make the sphere follow the end effector
sphere.position = robot.get_link_world_positions(link_id)