# print(robot.get_link_world_positions(flatten=False))
K = 5000 * np.identity(3)
# D = 2 * np.sqrt(K)
# D = np.zeros((3,3))
D = 100 * np.identity(3)
x_des = np.array([0.3, 0.0, 0.8])
x_des = np.array([0.52557296, 0.09732758, 0.80817658])
link_id = robot.get_link_ids('iiwa_link_ee')
for i in count():
# print(robot.get_link_world_positions(flatten=False))
# get state
q = robot.get_joint_positions()
dq = robot.get_joint_velocities()
x = robot.get_link_world_positions(link_id)
dx = robot.get_link_world_linear_velocities(link_id)
# get (linear) jacobian
J = robot.get_linear_jacobian(link_id, q)
# get coriolis, gravity compensation torques
torques = robot.get_coriolis_and_gravity_compensation_torques(q, dq)
# Impedance control: attractor point
F = K.dot(x_des - x) - D.dot(dx)
# F = -D.dot(dx)
tau = J.T.dot(F)
print(tau)
torques += tau
color=(0.1, 0.75, 0.1, 0.6))
ellipsoid_id = robot.draw_velocity_manipulability_ellipsoid(
link_id=-1, JJT=10 * velocity_manip[0:3, 0:3], color=(0.75, 0.1, 0.1, 0.6))
# Run simulator
for i in count():
robot.compute_and_draw_com_position(radius=0.03)
print("CoM: {}".format(robot.get_center_of_mass_position()))
# get current joint position
qt = robot.get_joint_positions()
# get center of mass jacobian
Jcom = robot.get_center_of_mass_jacobian(qt)
print("CoM velocity: {}".format(robot.get_center_of_mass_velocity()))
print("Jcom.dot(qt) = {}".format(Jcom.dot(robot.get_joint_velocities())))
# Get Inertia Matrix
# M = robot.get_mass_matrix(q=qt)
# print("M: {}".format(M))
# Tracking of CoM velocity manipulability
velocity_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom)
print("Mv: {}".format(velocity_manip[0:3, 0:3]))
# Plot current manipulability ellipsoid
if i % 10 == 0:
ellipsoid_id = robot.update_manipulability_ellipsoid(
link_id=-1,
ellipsoid_id=ellipsoid_id,
ellipsoid=10 * velocity_manip[:3, :3],