# load data with open(os.path.dirname(os.path.abspath(__file__)) + '/data.txt', 'rb') as f: positions = pickle.load(f) # set initial joint position robot.reset_joint_states(q=positions[0]) # draw a sphere at the position of the end-effector sphere = world.load_visual_sphere( position=robot.get_link_world_positions(link_id), radius=0.05, color=(1, 0, 0, 0.5)) sphere = Body(sim, body_id=sphere) # perform simulation for t in count(): # if no more joint positions, get out of the loop if t >= len(positions): break # set joint positions robot.set_joint_positions(positions[t], joint_ids=joint_ids) # make the sphere follow the end effector sphere.position = robot.get_link_world_positions(link_id) # step in simulation world.step(sleep_dt=1. / 240)
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) # set joint positions q = q[qIdx] + dq * dt robot.set_joint_positions(q, joint_ids=joint_ids) # step in simulation world.step(sleep_dt=dt)
# 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], color=(0.75, 0.1, 0.1, 0.6)) # Obtaining joint velocity command dq = robot.calculate_inverse_differential_kinematics_velocity_manipulability( Jcom, desired_velocity_manip, Km)[0] # Set joint position q = qt + dq * dt robot.set_joint_positions(q) world.step(sleep_dt=dt)
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)
class Environment: def __init__(self, spheres=[[0.09, -0.44, 0.715, 1]], cuboids=None): # Define size of the map self.res = 1. # Initialize XBox controller self.controller = XboxControllerInterface(use_thread=True, verbose=False) # Create simulator self.sim = Bullet() # create world self.world = BasicWorld(self.sim) # create robot self.robot = KukaIIWA(self.sim) # define start/end pt self.start_pt = [-0.75, 0., 0.75] self.end_pt = [0.75, 0., 0.75] # Initialize robot location self.send_robot_to(np.array(self.start_pt), dt=2) # Get via points from user via_pts = self.get_via_pts( ) # [[-0.19, -0.65, 0.77], [0.51, -0.44, 0.58]] self.motion_targets = [self.start_pt] + via_pts + [self.end_pt] def send_robot_to(self, location, dt): start = time.perf_counter() for _ in count(): joint_ids = self.robot.joints link_id = self.robot.get_end_effector_ids(end_effector=0) qIdx = self.robot.get_q_indices(joint_ids) x = self.robot.sim.get_link_world_positions(body_id=self.robot.id, link_ids=link_id) q = self.robot.calculate_inverse_kinematics(link_id, position=location) self.robot.set_joint_positions(q[qIdx], joint_ids) self.world.step() if time.perf_counter() - start >= dt: break def get_via_pts(self): print( "Move green dot to via point. Press 'A' to save. Press 'X' to finish." ) X_desired = [0., 0., 1.3] speed_scale_factor = 0.01 via_pts = [] sphere = None for _ in count(): last_trigger = self.controller.last_updated_button movement = self.controller[last_trigger] if last_trigger == 'LJ': X_desired[0] = X_desired[0] + round(movement[0], 1) * speed_scale_factor X_desired[1] = X_desired[1] + round(movement[1], 1) * speed_scale_factor elif last_trigger == 'RJ': X_desired[2] = X_desired[2] + round(movement[1], 1) * speed_scale_factor elif last_trigger == 'A' and movement == 1: print("Via point added") via_pts.append(X_desired) time.sleep(0.5) elif last_trigger == 'X' and movement == 1: self.world.sim.remove_body(sphere) print("Via point generation complete.") break if sphere: self.world.sim.remove_body(sphere) sphere = self.world.load_visual_sphere(X_desired, radius=0.01, color=(0, 1, 0, 1)) return via_pts
# IK using pybullet # ##################### # 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_positions(link_id, wrt_link_id) 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 robot.set_joint_positions(q[qIdx], joint_ids) # step in simulation world.step(sleep_dt=dt) # OPTION 2: using Jacobian and manual damped-least-squares IK ### elif solver_flag == 1: kp = 50 # 5 if velocity control, 50 if position control kd = 0 # 2*np.sqrt(kp) for _ in count(): # get current position in the task/operational space # x = robot.get_link_positions(link_id, wrt_link_id) # dx = robot.get_link_linear_velocities(link_id, wrt_link_id) x = robot.get_link_world_positions(link_id) dx = robot.get_link_world_linear_velocities(link_id)