# 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