class Robot: def __init__(self, sim, world): self.robot = KukaIIWA(sim) self.world = world # change the robot visual self.robot.change_transparency() self.robot.draw_link_frames(link_ids=[-1, 0]) self.start_pos = np.array([-0.75, 0., 0.75]) self.end_pos = np.array([0.75, 0., 0.75]) self.pid = {'kp': 100, 'kd': 0} self.ee_id = self.robot.get_end_effector_ids(end_effector=0) def send_robot_to(self, location): # define useful variables for IK dt = 1. / 240 link_id = self.robot.get_end_effector_ids(end_effector=0) joint_ids = self.robot.joints # actuated joint # joint_ids = joint_ids[2:] damping = 0.01 # for damped-least-squares IK qIdx = self.robot.get_q_indices(joint_ids) # for t in count(): # get current position in the task/operational space x = self.robot.sim.get_link_world_positions(body_id=self.robot.id, link_ids=link_id) dx = self.robot.get_link_world_linear_velocities(link_id) # Get joint configuration q = self.robot.sim.get_joint_positions(self.robot.id, self.robot.joints) # Get linear jacobian if self.robot.has_floating_base(): J = self.robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6] else: J = self.robot.get_linear_jacobian(link_id, q=q)[:, qIdx] # Pseudo-inverse Jp = self.robot.get_damped_least_squares_inverse(J, damping) # evaluate damped-least-squares IK dq = Jp.dot(self.pid['kp'] * (location - x) - self.pid['kd'] * dx) # set joint velocities # robot.set_joint_velocities(dq) # set joint positions q = q[qIdx] + dq * dt self.robot.set_joint_positions(q, joint_ids=joint_ids) return x def go_home(self): for t in count(): x = self.send_robot_to(self.start_pos) # step in simulation self.world.step() # Check if robot has reached the required position error = np.linalg.norm(self.start_pos - x) if error < 0.01 or t > 500: break def go_to_end(self): self.send_robot_to(self.end_pos)
# 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)
print("Joint ids: {}".format(robot.joints)) 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():