]) # 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 dq = Jp.dot(kp * (sphere.position - x) - kd * dx)
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)