# define useful variables for IK
dt = 1. / 240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
# joint_ids = joint_ids[2:]
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
# desired position
xd = np.array([0.5, 0., 0.5])
world.load_visual_sphere(xd, radius=0.05, color=(1, 0, 0, 0.5))
# change the robot visual
robot.change_transparency()
robot.draw_link_frames(link_ids=[-1, 0])
robot.draw_bounding_boxes(link_ids=joint_ids[0])
# robot.draw_link_coms([-1,0])
qIdx = robot.get_q_indices(joint_ids)
# OPTION 1: using `calculate_inverse_kinematics`###
if solver_flag == 0:
for _ in count():
# get current position in the task/operational space
x = robot.get_link_world_positions(link_id)
# print("(xd - x) = {}".format(xd - x))
# perform full IK
q = robot.calculate_inverse_kinematics(link_id, position=xd)
# set the joint positions
print("Q Indices: {}".format(robot.get_q_indices()))
print("Actuated joint ids: {}".format(robot.joints))
print("Link names: {}".format(robot.get_link_names(robot.joints)))
print("End-effector names: {}".format(
robot.get_link_names(robot.get_end_effector_ids())))
print("Floating base? {}".format(robot.has_floating_base()))
print("Total mass = {} kg".format(robot.mass))
print("")
print("Base name for IK: {}".format(base_name))
print("Link name for IK: {}".format(end_effector_name))
print("Chain: {}".format(chain_name))
print("")
robot.change_transparency()
robot.draw_link_frames([-1, 0])
robot.draw_bounding_boxes(joint_ids[0])
# robot.draw_link_coms([-1,0])
qIdx = robot.get_q_indices(joint_ids)
print(qIdx)
print(joint_ids)
#####################
# IK using pybullet #
#####################
# OPTION 1: using `calculate_inverse_kinematics`###
if solver_flag == 0:
for _ in count():
# # get current position in the task/operational space