import ikpy.utils.plot as plot_utils import matplotlib.pyplot as plt import numpy as np left_arm_chain = Chain(name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) fig, ax = plot_utils.init_3d_figure() left_arm_chain.plot([0, 0, 0, 0], ax) plt.show()
# init_position2 = le_arm_chain.forward_kinematics(init_servo_radians) # init_position = np.round(servo_range_to_xyz2(init_servo_values, current_servo_monotony), 2) # print("predicted_init_position: ", init_position) # print("proper init_position: ", target_position) except Exception as e: print("Exception: {}".format(str(e))) if center_init: print("Top position (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( init_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( init_position, np.eye(3))), ax, target=init_position) matplotlib.pyplot.show() print("Target angles (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( target_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( target_position, np.eye(3))), ax, target=target_position) matplotlib.pyplot.show()
for (i, angle) in enumerate(angles): angles[i] = math.degrees(angle) if (i == 0): angles[i] = angles[i] + 30 elif (i == 3 and angles[i] == 0): # FIXME: try and fix this angles[i] = angles[i] + 180 # if (angles[i] > 360 ): # angles[i] = angles[i] % 360 print("angle({}) = {} deg, {} pos".format(i, angles[i], int(angles[i] / 0.24))) arm.plot(arm.inverse_kinematics(target_frame), ax, target=target_vector) # check if reached ! # real_frame = arm.forward_kinematics(arm.inverse_kinematics(target_frame)) print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) # plot # # plt.xlim(-SCALER, SCALER) # plt.ylim(-SCALER, SCALER) plt.show() # sleep(2) # print("showing moving arm")
gait_sequence[[0, 2, 4, 6, 1, 3, 5, 7]], fmt='%3.1i', delimiter=',', newline=", ") #f.write("+") f.write("\n") f.close() # Create plot and image for each frame fig = plt.figure() ax = p3.Axes3D(fig) ax.set_box_aspect([3, 3 / 2, 1]) ax.set_xlim3d([-20, 10]) ax.set_xlabel('X') ax.set_ylim3d([-10, 10]) ax.set_ylabel('Y') ax.set_zlim3d([0.0, 10]) ax.set_zlabel('Z') right_arm.plot(right_arm_angles_raw, ax) right_leg.plot(right_leg_angles_raw, ax) left_arm.plot(left_arm_angles_raw, ax) left_leg.plot(left_leg_angles_raw, ax) figureName = "Nybble_" + str(frame) plt.savefig(figureName) plt.close() #plt.show()
for (i, angle) in enumerate(angles): angles[i] = math.degrees(angle) print("angle({}) = {} deg, {} pos".format(i, angles[i], int(angles[i] / 0.24))) 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(my_chain.inverse_kinematics(target_frame), ax, target=target_vector) print("-------yellow--------") # yellow # target_vector = [0.4, 0.8, 0] target_frame = np.eye(4) target_frame[:3, 3] = target_vector angles = my_chain.inverse_kinematics(target_frame) for (i, angle) in enumerate(angles): angles[i] = math.degrees(angle) print("angle({}) = {} deg, {} pos".format(i, angles[i], int(angles[i] / 0.24)))
] robo_chain = Chain(name="robo_chain", links=ulinks) robo_chain.forward_kinematics([0] * len(ulinks)) # test matrix, to check if defaults are as expected: look_straight = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] # translate in all directions tf1 = [[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 2], [0, 0, 0, 1]] # rotate along all axes tf2 = [[0.7, 0, 0, 2], [0, 0.7, 0, 2], [0, 0, 0.7, 2], [0, 0, 0, 1]] #robo__chain.inverse_kinematics(look_straight) import time #time.sleep(1000) if __name__ == '__main__': # show inverse kinematic import matplotlib.pyplot from mpl_toolkits.mplot3d import Axes3D for tf in [look_straight, tf1, tf2]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') robo_chain.plot(robo_chain.inverse_kinematics(tf), ax) matplotlib.pyplot.show() # close current plot, to show the next!
rotation=[0, 0, 0], ) ], active_links_mask=[False, True, True, True]) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') target_vector = [0.1, -0.2, 0.1] l_wrist_matrix = geometry_utils.to_transformation_matrix(target_vector) target_frame = np.eye(4) target_frame[:3, 3] = target_vector left_arm_start_position = left_arm_chain.forward_kinematics([0] * 4) right_arm_start_position = right_arm_chain.forward_kinematics([0] * 4) head_start_position = head_chain.forward_kinematics([0] * 3) left_leg_start_position = left_leg_chain.forward_kinematics([0] * 4) right_leg_start_position = right_leg_chain.forward_kinematics([0] * 4) print(right_arm_start_position) left_arm_chain.plot(left_arm_chain.inverse_kinematics(l_wrist_matrix), ax, target=target_vector) right_arm_chain.plot( right_arm_chain.inverse_kinematics(right_arm_start_position), ax) head_chain.plot(head_chain.inverse_kinematics(head_start_position), ax) left_leg_chain.plot(left_leg_chain.inverse_kinematics(left_leg_start_position), ax) right_leg_chain.plot( right_leg_chain.inverse_kinematics(right_leg_start_position), ax) matplotlib.pyplot.show()
class Control: """ Realization of continuous actions, from world model to desired world. """ def __init__(self, init_world_model): self.control_world_model = init_world_model.current_world_model.control self.arm_url = init_world_model.current_world_model.url["arm"] self.cm_to_servo_polynomial_fitter = \ joblib.load(init_world_model.current_world_model.control["cm_to_servo_polynomial_fitter"]["file_path"]) self.init_position = np.array(self.control_world_model["init_position"] ) * self.control_world_model["scale"] self.link_bounds = tuple( np.radians( np.array([ -self.control_world_model["angle_degree_limit"], self.control_world_model["angle_degree_limit"] ]))) # In degrees self.le_arm_chain = Chain( name=self.control_world_model["chain_name"], active_links_mask=self.control_world_model["active_links_mask"], links=[ URDFLink( name="link6", translation_vector=np.array( self.control_world_model["link_lengths"]["link6"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link6"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link6"]), bounds=self.link_bounds), URDFLink( name="link5", translation_vector=np.array( self.control_world_model["link_lengths"]["link5"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link5"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link5"]), bounds=self.link_bounds), URDFLink( name="link4", translation_vector=np.array( self.control_world_model["link_lengths"]["link4"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link4"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link4"]), bounds=self.link_bounds), URDFLink( name="link3", translation_vector=np.array( self.control_world_model["link_lengths"]["link3"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link3"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link3"]), bounds=self.link_bounds), URDFLink( name="link2", translation_vector=np.array( self.control_world_model["link_lengths"]["link2"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link2"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link2"]), bounds=self.link_bounds), URDFLink( name="link1", translation_vector=np.array( self.control_world_model["link_lengths"]["link1"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link1"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link1"]), bounds=self.link_bounds) ]) def xyz_to_servo_range(self, xyz, current_servo_monotony): """ Converts 3D cartesian centimeter coordinates to servo values in [500, 2500]. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param current_servo_monotony: List of 6 positive or negative servo rotation directions. :return: List of 6 servo values in [500, 2500]. """ k = self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(xyz, np.eye(3))) k = np.multiply(k, np.negative(current_servo_monotony)) return self.radians_to_servo_range(k) def servo_range_to_xyz(self, servo_range, current_servo_monotony): """ Converts servo values in [500, 2500] to 3D cartesian centimeter coordinates. :param servo_range: List of 6 servo values in [500, 2500]. :param current_servo_monotony: List of 6 positive or negative servo rotation directions. :return: Array of 3 elements of a 3D cartesian systems of centimeters. """ return geometry_utils.from_transformation_matrix( self.le_arm_chain.forward_kinematics( np.multiply(self.servo_range_to_radians(servo_range), np.negative(current_servo_monotony)), ))[0][:3] @staticmethod def servo_range_to_radians(x, x_min=500.0, x_max=2500.0, scaled_min=(-np.pi / 2.0), scaled_max=(np.pi / 2.0)): """ Converts servo values in [500, 2500] to angle radians. :param x: List of 6 servo values. :param x_min: Scalar float, minimum servo value of 90 degrees angle (default = 500). :param x_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500). :param scaled_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2). :param scaled_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2). :return: List of 6 angles in radians. """ x_std = (np.array(x) - x_min) / (x_max - x_min) return x_std * (scaled_max - scaled_min) + scaled_min @staticmethod def radians_to_servo_range(x, x_min=(-np.pi / 2.0), x_max=(np.pi / 2.0), scaled_min=500.0, scaled_max=2500.0): """ Converts angle radians to servo values in [500, 2500]. :param x: List of 6 angles in radians. :param x_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2). :param x_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2). :param scaled_min: Scalar float, minimum servo value of 90 degrees angle (default = 500). :param scaled_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500). :return: List of 6 servo values. """ x_std = (np.array(x) - x_min) / (x_max - x_min) return (np.round(x_std * (scaled_max - scaled_min) + scaled_min, 0)).astype(int) def get_kinematic_angle_trajectory(self, from_angle_radians_in, to_angle_radians_in, servo_monotony, steps=10): """ Creates a discrete end-effector trajectory, using radians. :param from_angle_radians_in: Current servo angles, list of 6 angles in radians. :param to_angle_radians_in: Desired servo angles, list of 6 angles in radians. :param servo_monotony: List of 6 positive or negative servo rotation directions. :param steps: Scalar integer, the total steps for the end effector trajectory. :return: List of end-effector radian trajectory steps. """ assert self.control_world_model[ "min_steps"] < steps < self.control_world_model["max_steps"] from_angle_radians = np.multiply(from_angle_radians_in, servo_monotony) to_angle_radians = np.multiply(to_angle_radians_in, servo_monotony) step_angle_radians = [] for index in range(len(to_angle_radians_in)): step_angle_radians.append( (from_angle_radians[index] - to_angle_radians[index]) / float(steps)) angle_trajectory = [] step_angle_radians = np.array(step_angle_radians) current_angles = np.array(from_angle_radians) # angle_trajectory.append(current_angles) for _ in range(steps): current_angles = np.add(current_angles, step_angle_radians) angle_trajectory.append(current_angles) return angle_trajectory def get_servo_range_trajectory(self, from_servo_range_in, to_servo_range_in, steps=10): """ Creates a discrete end-effector trajectory, using servo values. :param from_servo_range_in: Current servo values, list of 6 values in [500, 2500]. :param to_servo_range_in: Desired servo values, list of 6 values in [500, 2500]. :param steps: Scalar integer, the total steps for the end effector trajectory. :return: List of end-effector servo value trajectory steps. """ assert self.control_world_model[ "min_steps"] < steps < self.control_world_model["max_steps"] from_servo_range = np.array(from_servo_range_in) to_servo_range = np.array(to_servo_range_in) if self.control_world_model["verbose"]: print("from_servo_range: ", from_servo_range) print("to_servo_range: ", to_servo_range) step_servo_range = [] for index in range(len(to_servo_range)): step_servo_range.append( (to_servo_range[index] - from_servo_range[index]) / float(steps)) if self.control_world_model["verbose"]: print("step_servo_range: ", step_servo_range) servo_range_trajectory = [] step_servo_range = np.array(step_servo_range) current_servo_range = np.array(from_servo_range) for _ in range(steps): current_servo_range = np.add(current_servo_range, step_servo_range) servo_range_trajectory.append(current_servo_range) return np.array(np.round(servo_range_trajectory, 0)).astype(int) def initialize_arm(self, last_servo_values): """ Moves the end-effector to the (0, 0, 0) position of the 3d cartesian. :param last_servo_values: List of the current arm servo positions. :return: True if succeeded. """ action_successful = False target_position = np.array( self.init_position) * self.control_world_model["scale"] action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm initialized") return action_successful def move_arm_above_xyz(self, xyz, last_servo_values, height): """ Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param last_servo_values: List of the current arm servo positions. :param height: Scalar positive float. Desired centimeters above xyz, on the z axis. :return: True if succeeded. """ action_successful = False xyz[2] = height target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm above object") return action_successful def move_arm_up(self, last_servo_values, height): """ Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high. :param last_servo_values: List of the current arm servo positions. :param height: Scalar positive float. Desired centimeters above xyz, on the z axis. :return: True if succeeded. """ action_successful = False xyz = np.round( self.servo_range_to_xyz( last_servo_values, self.control_world_model["current_servo_monotony"]), 2) print("last_servo_xyz", xyz) xyz[2] = height target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm up") return action_successful def move_arm_to_object(self, xyz, last_servo_values): """ Moves the end-effector to the object's position of the 3d cartesian. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param last_servo_values: List of the current arm servo positions. :return: True if succeeded. """ action_successful = False target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm to object") return action_successful def close_hand(self, object_side_length): """ Closes the gripper enough, to grip an object of a specific length in cm. :param object_side_length: Scalar float, object width in centimeters. :return: True if succeeded. """ action_successful = False closed_length = object_side_length * self.control_world_model[ "closed_hand_distance_ratio"] servo_range = int(self.cm_to_servo_polynomial_fitter(closed_length)) if self.control_world_model["verbose"]: print("cm: {}, predicted servo value: {}".format( closed_length, servo_range)) if self.control_world_model["send_requests"]: action_successful = self.send_restful_servo_range( self.control_world_model["gripping_gripper_servo"], servo_range) print("=== Gripper closed") return action_successful def open_hand(self, object_side_length): """ Opens the gripper enough, to fit an object of a specific length in cm. :param object_side_length: Scalar float, object width in centimeters. :return: True if succeeded. """ action_successful = False opened_length = object_side_length * self.control_world_model[ "opened_hand_distance_ratio"] servo_range = int(self.cm_to_servo_polynomial_fitter(opened_length)) if self.control_world_model["verbose"]: print("cm: {}, predicted servo value: {}".format( opened_length, servo_range)) if self.control_world_model["send_requests"]: action_successful = self.send_restful_servo_range( self.control_world_model["gripping_gripper_servo"], servo_range) print("=== Gripper opened") return action_successful def send_restful_servo_range(self, servo, in_range): """ Sends a direct servo value in [500, 2500], to a specific servo in [1, 6]. :param servo: Scalar integer, the servo id in [1, 6]. :param in_range: Scalar integer, servo value in [500, 2500]. :return: True if succeeded. """ action_successful = False url = self.control_world_model["base_put_url"].format( self.arm_url, servo, in_range) requests.put(url, data="") time.sleep(self.control_world_model["command_delay"]) action_successful = True return action_successful def send_restful_trajectory_requests(self, kinematic_servo_range_trajectory): """ Sends a full servo value trajectory of discrete steps, to the arm. :param kinematic_servo_range_trajectory: :return: True if succeeded. """ action_successful = False servo_mask = self.control_world_model[ "active_links_mask"] # servo mask for step in kinematic_servo_range_trajectory: for i in range(len(step)): if servo_mask[i]: servo_value = step[i] current_servo = self.control_world_model["servo_count"] - i url = self.control_world_model["base_put_url"].format( self.arm_url, current_servo, servo_value) if self.control_world_model["verbose"]: print(url) try: r = requests.put(url, data="") if r.status_code != 200: break except Exception as e: print("Exception: {}".format(str(e))) time.sleep(self.control_world_model["command_delay"]) if self.control_world_model["verbose"]: print("") action_successful = True return action_successful def move_arm(self, target_position, last_servo_locations, trajectory_steps=-1): """ Gradually moves the end-effector of the robotic arm, from the latest known servo positions, to a desired 3D centimeter cartesian position. :param target_position: List of 3 values, the desired end-effector, 3D centimeter cartesian position. :param last_servo_locations: List of the latest 6 servo values in [500, 2500]. :param trajectory_steps: Scalar integer, the total steps for the end effector trajectory. :return: True if successfully move the arm. """ action_successful = False last_servo_values = self.init_position if trajectory_steps == -1: trajectory_steps = self.control_world_model["trajectory_steps"] # TODO: move last position to world model if self.control_world_model[ "detect_last_position"]: # TODO: get last servo values, world may be old...? last_servo_values = last_servo_locations try: if self.control_world_model["send_requests"]: url = "http://{}/".format(self.arm_url) r = requests.get(url, data="") if r.status_code == 200: result = r.json()["variables"] last_servo_values = np.array([ result["servo6"], result["servo5"], result["servo4"], result["servo3"], result["servo2"], result["servo1"] ]) if self.control_world_model["verbose"]: print("last_servo_values: ", last_servo_values) print( "last_servo_xyz", np.round( self.servo_range_to_xyz( last_servo_values, self.control_world_model[ "current_servo_monotony"]), 2)) except Exception as e_pos: print("Exception: {}".format(str(e_pos))) if self.control_world_model["center_init"]: if self.control_world_model["verbose"]: print( "Top position (radians): ", self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( self.init_position, np.eye(3)))) if self.control_world_model["show_plots"]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( self.init_position, np.eye(3))), ax, target=self.init_position) matplotlib.pyplot.show() init_position2 = self.init_position init_angle_radians2 = self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(init_position2, np.eye(3))) from_servo_range = self.radians_to_servo_range(init_angle_radians2) if self.control_world_model["detect_last_position"]: from_servo_range = last_servo_values to_servo_range = self.xyz_to_servo_range( target_position, self.control_world_model["current_servo_monotony"]) kinematic_servo_range_trajectory = self.get_servo_range_trajectory( from_servo_range, to_servo_range, trajectory_steps) if self.control_world_model["verbose"]: print( "init_angle_radians2: {}, from_servo_range: {}, to_servo_range: {}, servo_range_trajectory: {}" .format(init_angle_radians2, from_servo_range, to_servo_range, kinematic_servo_range_trajectory)) if self.control_world_model["show_plots"]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( target_position, np.eye(3))), ax, target=target_position) matplotlib.pyplot.show() if self.control_world_model["send_requests"]: action_successful = self.send_restful_trajectory_requests( kinematic_servo_range_trajectory) return action_successful
name="joint1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="joint2", translation_vector=[0, 0, 0.075], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="joint3", translation_vector=[0.045, 0, 0.802], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="joint4", translation_vector=[-0.045, 0, 0.746], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) my_chain.plot( my_chain.inverse_kinematics([[1, 0, 0, -0.3], [0, 1, 0, 0.3], [0, 0, 1, 1], [0, 0, 0, 1]]), ax) #my_chain.plot([0,0,0],ax) matplotlib.pyplot.show()
name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) target_position = [2, 2, 2] print( left_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(target_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') left_arm_chain.plot(left_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( [2, 2, 2], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])), ax, target=target_position) matplotlib.pyplot.show()
orientation=[0, 0, 0], rotation=[0, 0, 1], ), URDFLink( name="second_link", translation_vector=[1, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], ) ]) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') target_vector = [1, 0, 0] target_frame = np.eye(4) target_frame[:3, 3] = target_vector target_frame print("The angles of each joints are : ", test_link.inverse_kinematics(target_frame)) real_frame = test_link.forward_kinematics( test_link.inverse_kinematics(target_frame)) print("--> Computed position vector : %s" % real_frame[:3, 3]) print("--> original position vector : %s" % target_frame[:3, 3]) ax = plot_utils.init_3d_figure() test_link.plot(test_link.inverse_kinematics(target_frame), ax, target=target_vector) matplotlib.pyplot.xlim(-1, 2) matplotlib.pyplot.ylim(-1, 2) matplotlib.pyplot.show()
class Wobbler(object): def __init__(self): self._done = False self._head = baxter_interface.Head() self._left_arm = baxter_interface.limb.Limb('left') self._right_arm = baxter_interface.limb.Limb('right') print(self._left_arm.joint_names()) print(self._left_arm.joint_angles()) print(self._left_arm.joint_velocities()) print(self._left_arm.joint_efforts()) print(self._left_arm.endpoint_pose()) print(self._left_arm.endpoint_velocity()) print(self._left_arm.endpoint_effort()) self._left_arm_chain = Chain( name='left_arm', active_links_mask=[ False, False, True, True, True, True, True, True, True, False ], links=[ OriginLink(), URDFLink( name="left_torso_arm_mount", translation_vector=[0.024645, 0.219645, 0.118588], orientation=[0, 0, 0.7854], rotation=[0, 0, 1], ), URDFLink(name="left_s0", translation_vector=[0.055695, 0, 0.011038], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-1.70167993878, 1.70167993878)), URDFLink(name="left_s1", translation_vector=[0.069, 0, 0.27035], orientation=[-1.57079632679, 0, 0], rotation=[0, 0, 1], bounds=(-2.147, 1.047)), URDFLink(name="left_e0", translation_vector=[0.102, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.05417993878, 3.05417993878)), URDFLink(name="left_e1", translation_vector=[0.069, 0, 0.26242], orientation=[-1.57079632679, -1.57079632679, 0], rotation=[0, 0, 1], bounds=(-0.05, 2.618)), URDFLink(name="left_w0", translation_vector=[0.10359, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.059, 3.059)), URDFLink(name="left_w1", translation_vector=[0.01, 0, 0.2707], orientation=[-1.57079632679, -1.57079632679, 0], rotation=[0, 0, 1], bounds=(-1.57079632679, 2.094)), URDFLink(name="left_w2", translation_vector=[0.115975, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.059, 3.059)), URDFLink( name="left_hand", translation_vector=[0, 0, 0.11355], orientation=[0, 0, 0], rotation=[0, 0, 1], ) ]) #self._left_arm_chain = ikpy.chain.Chain.from_urdf_file("/home/jbunker/ros_ws/src/baxter_common/baxter_description/urdf/baxter.urdf") # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit") def clean_shutdown(self): """ Exits example cleanly by moving head to neutral position and maintaining start state """ print("\nExiting example...") if self._done: self.set_neutral() if not self._init_state and self._rs.state().enabled: print("Disabling robot...") self._rs.disable() def set_neutral(self): """ Sets the head back into a neutral pose """ self._head.set_pan(0.0) def testJoint(self): self.set_neutral() print(self._left_arm.joint_names()) target = (self._right_arm.joint_names())[0] print("targeting joint {0}".format(target)) print("starting angle {0}".format(self._right_arm.joint_angle(target))) print("starting velocity {0}".format( self._right_arm.joint_velocity(target))) print("starting effort {0}".format( self._right_arm.joint_effort(target))) command_rate = rospy.Rate(1) control_rate = rospy.Rate(50) commandDictLeft = {} for n in self._left_arm.joint_names(): commandDictLeft[n] = 0. commandDictRight = {} for n in self._right_arm.joint_names(): commandDictRight[n] = 0. start = rospy.get_time() while not rospy.is_shutdown() and (rospy.get_time() - start < 0.1): self._left_arm.set_joint_positions(commandDictLeft) self._right_arm.set_joint_positions(commandDictRight) control_rate.sleep() print("zeroing ended, recording starting waypoint...") i = 0 while (i < 3): state = jointStates.get() print(i) i += 1 originP = [0.064027, 0.259027, 0.08] target = np.asarray([[1, 0, 0, .7], [0, 1, 0, .0], [0, 0, 1, .5], [0, 0, 0, 1]]) angles = self._left_arm.joint_angles() order = [ "origin", "left_torso_arm_mount", "left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2", "left_hand" ] angles = properOrder(self._left_arm.joint_angles(), order) print(angles) print(target) print(self._left_arm_chain) solution = ikpy.inverse_kinematics.inverse_kinematic_optimization( chain=self._left_arm_chain, target_frame=target, starting_nodes_angles=angles) print(solution) inp = convertToDict(order, solution) i = 0 while (getDiff(angles, solution) > 0.01 and i < 1000): print(getDiff(angles, solution)) inp = convertToDict(order, solution) print(inp) self._left_arm.set_joint_positions(convertToDict(order, solution)) #self._left_arm.set_joint_positions(commandDictLeft) control_rate.sleep() angles = properOrder(self._left_arm.joint_angles(), order) #print(angles) i += 1 print(self._left_arm_chain) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') self._left_arm_chain.plot( ikpy.inverse_kinematics.inverse_kinematic_optimization( chain=self._left_arm_chain, target_frame=target, starting_nodes_angles=angles), ax) matplotlib.pyplot.show() #t_angles = self.left_arm_chain.inverse_kinematics(target=target_frame,initial_position=self._left_arm.joint_positions())) self._done = True rospy.signal_shutdown("Example finished.")
translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) left_arm_chain.plot(left_arm_chain.joints, ax) # left_arm_chain.plot(left_arm_chain.inverse_kinematics([ # [1, 0, 0, 2], # [0, 1, 0, 2], # [0, 0, 1, 2], # [0, 0, 0, 1] # ]), ax) matplotlib.pyplot.show()