def get_angles(xyz): x = xyz[0] y = xyz[1] z = xyz[2] my_chain = ikpy.chain.Chain.from_urdf_file("robot_2link.URDF") target_vector = [x, y, z] target_frame = np.eye(4) target_frame[:3, 3] = target_vector #print("The angles of each joints are : ", my_chain.inverse_kinematics(target_frame)) real_frame = my_chain.forward_kinematics( my_chain.inverse_kinematics(target_frame)) #print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) ax = plot_utils.init_3d_figure() my_chain.plot([0, 0, 0, 0, 0], ax, target=target_vector) my_chain.plot(my_chain.inverse_kinematics(target_frame), ax, target=target_vector) angles = my_chain.inverse_kinematics(target_frame) angles[1] = (angles[1] * 180) / 3.14 angles[2] = (angles[2] * 180) / 3.14 angles[3] = (angles[3] * 180) / 3.14 #print(angles) #plt.xlim(-1,1) #plt.ylim(-1,1) #plt.show() return angles
def get_angles(): my_chain = ikpy.chain.Chain.from_urdf_file("robot_2link.URDF") target_vector = [0.6, 0.4, 0.3] target_frame = np.eye(4) target_frame[:3, 3] = target_vector print("The angles of each joints are : ", my_chain.inverse_kinematics(target_frame)) real_frame = my_chain.forward_kinematics( my_chain.inverse_kinematics(target_frame)) print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) import matplotlib.pyplot as plt ax = plot_utils.init_3d_figure() my_chain.plot([0, 0, 0, 0, 0], ax, target=target_vector) my_chain.plot(my_chain.inverse_kinematics(target_frame), ax, target=target_vector) angles = my_chain.inverse_kinematics(target_frame) #plt.xlim(-1,1) #plt.ylim(-1,1) #plt.show() return angles
def test_ergo(resources_path, interactive): a = chain.Chain.from_urdf_file(resources_path + "/testbot.urdf", base_elements=["base"]) print(a) target = [0.15, 0.15, 0.1] #target = [0.07, 0.0, 0.0] frame_target = np.eye(4) frame_target[:3, 3] = target joints = [0] * len(a.links) print(joints) ik = a.inverse_kinematics(frame_target, initial_position=joints) print("The angles of each joints are : ", a.inverse_kinematics(frame_target)) arr = a.inverse_kinematics(frame_target) print("Angle 0: ", math.degrees(arr[0])) print("Angle 1: ", math.degrees(arr[1])) print("Angle 2: ", math.degrees(arr[2])) print("Angle 3: ", math.degrees(arr[3])) print("Angle 4: ", math.degrees(arr[4])) brr = a.forward_kinematics(arr) print("Computed position vector: ", brr[:3, 3]) ax = plot_utils.init_3d_figure() a.plot(ik, ax, target=target) plt.savefig("out/ergo.png") if interactive: plot_utils.show_figure()
def setUp(self): if plot: self.ax = plot_utils.init_3d_figure() self.chain1 = chain.Chain.from_urdf_file(params.resources_path + "/poppy_torso.URDF", base_elements=["base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y"], last_link_vector=[0, 0.18, 0], active_links_mask=[False, False, False, False, True, True, True, True, True]) self.joints = [0] * len(self.chain1.links) self.joints[-4] = 0 self.target = [0.1, -0.2, 0.1] self.frame_target = np.eye(4) self.frame_target[:3, 3] = self.target
def IK(target_frame): ax = plot_utils.init_3d_figure() my_chain = ikpy.chain.Chain.from_urdf_file("urdf/01-myfirst.urdf") print(my_chain) jnts = my_chain.inverse_kinematics(target_frame) my_chain.plot(jnts, ax, target=target_frame[:3, 3]) plt.show() return jnts
def test(): my_chain = ikpy.chain.Chain.from_urdf_file("poppy_ergo.URDF") target_vector = [0.1, -0.2, 0.1] target_frame = np.eye(4) target_frame[:3, 3] = target_vector print("The angles of each joints are : ", my_chain.inverse_kinematics(target_frame)) ax = plot_utils.init_3d_figure() my_chain.plot(my_chain.inverse_kinematics(target_frame), ax, target=target_vector, show=True) plt.xlim(-10, 10) plt.ylim(-10, 10)
def test_ergo(resources_path, interactive): a = chain.Chain.from_urdf_file(resources_path + "/poppy_ergo.URDF") target = [0.1, -0.2, 0.1] frame_target = np.eye(4) frame_target[:3, 3] = target joints = [0] * len(a.links) ik = a.inverse_kinematics(frame_target, initial_position=joints) ax = plot_utils.init_3d_figure() a.plot(ik, ax, target=target) plt.savefig("out/ergo.png") if interactive: plot_utils.show_figure()
def test_ergo(self): a = chain.Chain.from_urdf_file(params.resources_path + "/poppy_ergo.URDF") target = [0.1, -0.2, 0.1] frame_target = np.eye(4) frame_target[:3, 3] = target joints = [0] * len(a.links) ik = a.inverse_kinematics(frame_target, initial_position=joints) if plot: ax = plot_utils.init_3d_figure() if plot: a.plot(ik, ax, target=target) plot_utils.show_figure()
def setUp(self): self.ax = plot_utils.init_3d_figure() self.chain1 = chain.Chain.from_urdf_file( params.resources_path + "/poppy_torso.URDF", base_elements=[ "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y" ], last_link_vector=[0, 0.18, 0], active_links_mask=[ False, False, False, False, True, True, True, True, True ]) self.joints = [0] * len(self.chain1.links) self.joints[-4] = 0 self.target = [0.1, -0.2, 0.1] self.frame_target = np.eye(4) self.frame_target[:3, 3] = self.target
def test_urdf_chain(resources_path, interactive): """Test that we can open chain from a URDF file""" chain1 = chain.Chain.from_urdf_file(os.path.join(resources_path, "poppy_torso.URDF"), base_elements=[ "base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y" ], last_link_vector=[0, 0.18, 0], active_links_mask=[ False, False, False, False, True, True, True, True, True ]) joints = [0] * len(chain1.links) joints[-4] = 0 ax = plot_utils.init_3d_figure() chain1.plot(joints, ax) plt.savefig("out/chain1.png") if interactive: plt.show()
#Get physical chain: my_chain = ikpy.chain.Chain.from_urdf_file("./resources/BA006N.urdf") #CONFIGS: NUM_OF_JOINTS = 7 NUM_OF_RUNS = int(1e7) step = 30 to_z_mesh = range(0, 180+step, step) to_x_mesh = range(-180, 180, step) TO_Z_MESH, TO_X_MESH = np.meshgrid(to_z_mesh, to_x_mesh) WANTED_PAIR_deg = np.array([TO_X_MESH.reshape(-1), TO_Z_MESH.reshape(-1)]).T WANTED_PAIR = np.array([TO_X_MESH.reshape(-1), TO_Z_MESH.reshape(-1)]).T*np.pi/180 PLOT = True PLOT = False plt.ion() ax = plot_utils.init_3d_figure() #Check first: self_check() #prepare: real_frames = np.empty(shape=(NUM_OF_RUNS, NUM_OF_JOINTS,4,4)) six_axis = np.random.random((NUM_OF_RUNS, NUM_OF_JOINTS))*720-360 pairs = np.empty(shape=(NUM_OF_RUNS, 2)) densities = np.empty(shape=(NUM_OF_RUNS, 1)) #and run: pairs, densities = random_pose_sample(six_axis, pairs, densities) #For output pose nearest_to_anchor = [] for i in WANTED_PAIR_deg: nearest_to_anchor.append(np.abs((pairs-i)).sum(axis=1).argmin())
Moves the robot to standard idle position, make it compliant, then close the connections. """ print('Shutting down the robot...') self.init_angles() self.robot.compliant = True self.robot.close() print('Robotic arm shut down. Power source can now be safely unplugged.') if __name__ == '__main__': # A basic demonstration, presenting various features available with this class # Full robot arm = GripperArm('full') ax = plot_utils.init_3d_figure() # Reach first target, then close gripper target = [0.215, -0.13, 0.3] (fa, ma) = arm.goto_target(target, 1.5, wait = True) th_epos = arm.chain.forward_kinematics(fa)[:3, 3] current_epos = arm.get_endpoint_position() print('\nTheoretical error to target [m]: {}\nActual error to target [m]: {}'.format(norm(target - th_epos), norm(target - current_epos) ) ) arm.chain.plot(fa, ax, target = target) arm.set_clamp_position(25, 1, wait = True) # Reach second target, then open gripper target = [0.22, -0.09, 0.24] (fa, ma) = arm.goto_target(target, 1.5, wait = True) th_epos = arm.chain.forward_kinematics(fa)[:3, 3] current_epos = arm.get_endpoint_position()