class ArmIK: def __init__(self): self.chain = Chain( name="pupper_ergo", links=[ OriginLink(), URDFLink( name="twistybase", translation_vector=[0, 0, 0.03], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-90, 90), ), URDFLink( name="shoulder", translation_vector=[0, 0, 0.0285], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="elbow", translation_vector=[0, 0, 0.085], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="tip", translation_vector=[0, 0.155, 0], orientation=[1.57, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), ], ) @staticmethod def _ik2pupperarm(joints): return np.rad2deg([joints[1], -joints[2], -joints[3], 0.0]) @staticmethod def _fk2pupperarm(joints): return np.deg2rad([joints[1], -joints[2], -joints[3], 0.0, 0.0]) @staticmethod def _makeMoveCmd(degs): print(f"move([{','.join([str(x) for x in np.around(degs, 2)])}])") def pos2joints(self, target): joints = self.chain.inverse_kinematics(target) return self._ik2pupperarm(joints) def joints2pos(self, joints): joints = self._fk2pupperarm(joints) return self.chain.forward_kinematics(joints)[:3, 3]
class Arm: def __init__(self, direction): self.state = [0, 0, 0, 0, 0] if direction == "left": sign = -1 else: sign = 1 self.chain = Chain( name=direction + "_arm", links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[8 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1], ), URDFLink(name="backarm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(90 * sign)], rotation=[0, 0, 1]), URDFLink(name="forearm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1]), URDFLink(name="hand", translation_vector=[9 * sign, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), ]) def get_positions(self): rv = [] positions = self.chain.forward_kinematics(self.state, full_kinematics=True) for position in positions[1:]: xyEtc = position[:, 3] x = xyEtc[0] y = xyEtc[1] rv.append((x, y)) return rv def go_directly_to_position(self, x, y, book): rad = math.radians(book.openness / 2) if (x < book.center_x_cm): rad *= -1 joint_positions = self.chain.inverse_kinematics( target_position=[x, y, 0], target_orientation=[0, 0, 0.2]) self.state = joint_positions
class KSOLVER: def __init__(self, *args, **kwargs): self.chain = Chain( name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder_left", translation_vector=[0.13182, -2.77556e-17, 0.303536], orientation=[-0.752186, -0.309384, -0.752186], rotation=[0.707107, 0, 0.707107], ), URDFLink( name="proximal_left", translation_vector=[-0.0141421, 0, 0.0424264], orientation=[-7.77156e-16, 7.77156e-16, 5.55112e-17], rotation=[-0.707107, 0, 0.707107], ), URDFLink( name="distal_left", translation_vector=[0.193394, -0.0097, 0.166524], orientation=[1.66533e-16, -7.21645e-16, 3.88578e-16], rotation=[0, -1, 0], ) ]) def solve(self, wrist_postion): """Performs inverse kinematics and returns joint angles in rads For now uses one target vector at the wrist position for inverse kinematics. For the future, should use additional target vectors and match the elbow """ # TODO: Fix coordinate system transform for extra degree of freedom rotation # Setup target vector with target position at wrist target_vector = np.array( [wrist_postion[0], wrist_postion[1], wrist_postion[2]]) target_frame = np.eye(4) target_frame[:3, 3] = target_vector joint_angles = self.chain.inverse_kinematics(target_frame)[1:3] print('Joint angles:', joint_angles) print('Computed position vector:', self.chain.forward_kinematics(self.chain.inverse_kinematics(target_frame))[:3, 3], \ 'Original position vector:', target_frame[:3, 3]) joints = self.chain.inverse_kinematics(target_frame)[1:3] return np.array([0, joints[1]])
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") # new_target_vector = [x, y, 0] # target_frame[:3, 3] = new_target_vector # copy target vector
class InverseKinematics(): def __init__(self): # Second robo, with dynamixel self.armLength = 4 self.smallArmLength = 4 lim = math.pi / 4 # lim = 7*math.pi/9 self.chain = Chain( name='arm', links=[ OriginLink(), URDFLink(name="base", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-math.pi, math.pi)), URDFLink(name="first", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="second", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink(name="third", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="fourth", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink( name="tip", translation_vector=[0, 0, self.smallArmLength], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) self.xMin = -3 * self.armLength - self.smallArmLength self.xMax = 3 * self.armLength + self.smallArmLength self.yMin = -3 * self.armLength - self.smallArmLength self.yMax = 3 * self.armLength + self.smallArmLength self.zMin = 0 self.zMax = 3 * self.armLength + self.smallArmLength self.radToDegreeFactor = 180 / math.pi self.degreeToRadFactor = math.pi / 180 self.numberOfLinks = len(self.chain.links) # def __init__(self): # # First robo, with MG995 Servos # self.armLength = 10 # self.smallArmLength = 5 # self.chain = Chain(name='arm', links=[ # OriginLink( # ), # URDFLink( # name="base", # translation_vector=[0, 0, 0], # orientation=[0, 0, 0], # rotation=[0, 0, 1], # bounds=(-math.pi,math.pi) # ), # URDFLink( # name="first", # translation_vector=[0, 0, 0], # orientation=[0, 0, 0], # rotation=[0, 1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="second", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, -1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="third", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, 1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="fourth", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, -1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="tip", # translation_vector=[0, 0, self.smallArmLength], # orientation=[0, 0, 0], # rotation=[0, 0, 0], # ) # ]) # self.xMin = -3*self.armLength - self.smallArmLength # self.xMax = 3*self.armLength + self.smallArmLength # self.yMin = -3*self.armLength - self.smallArmLength # self.yMax = 3*self.armLength + self.smallArmLength # self.zMin = 0 # self.zMax = 3*self.armLength + self.smallArmLength # self.radToDegreeFactor = 180 / math.pi # self.degreeToRadFactor = math.pi / 180 # self.numberOfLinks = len(self.chain.links) def __del__(self): pass def getPosition(self, angles, allLinks=False): anglesRad = [] if not allLinks: anglesRad.append(0) for angle in angles: anglesRad.append(angle * self.degreeToRadFactor) if not allLinks: anglesRad.append(0) # print("anglesRad: {}".format(anglesRad)) calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] calcPosPerc = [] calcPosPerc.append( myutils.map(calcPos[0], self.xMin, self.xMax, -100, 100)) calcPosPerc.append( myutils.map(calcPos[1], self.yMin, self.yMax, -100, 100)) calcPosPerc.append( myutils.map(calcPos[2], self.zMin, self.zMax, 0, 100)) return calcPosPerc def getAngles(self, pos): if pos[0] == 0: pos[0] = 0.01 if pos[1] == 0: pos[1] = 0.01 if pos[2] == 0: pos[2] = 0.01 pos[0] = myutils.map(pos[0], 0, 100, self.xMin, self.xMax) pos[1] = myutils.map(pos[1], 0, 100, self.yMin, self.yMax) pos[2] = myutils.map(pos[2], 0, 100, self.zMin, self.zMax) anglesRad = self.chain.inverse_kinematics(pos) angles = anglesRad * self.radToDegreeFactor calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] calcPosPerc = self.getPosition(angles, True) newAngles = [] for i in range(1, self.numberOfLinks - 1): # newAngles.append(math.floor(angles[i])) newAngles.append(angles[i]) outOfRange = False for i in range(len(calcPosPerc)): if abs(calcPosPerc[i] - pos[i]) > 2: outOfRange = True break log("{} calcPosPerc {}".format(outOfRange, calcPosPerc), "OK") print("Angles are: {}".format(newAngles)) return newAngles, outOfRange def getAnglesRaw(self, pos): if pos[0] == 0: pos[0] = 0.01 if pos[1] == 0: pos[1] = 0.01 if pos[2] == 0: pos[2] = 0.01 corrPos = [] corrPos.append(pos[0] * self.xMax / 100) corrPos.append(pos[1] * self.yMax / 100) corrPos.append(pos[2] * self.zMax / 100) anglesRad = self.chain.inverse_kinematics(corrPos) angles = anglesRad * self.radToDegreeFactor startTime = time.time() calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] log("elapsed timeO: {}".format(time.time() - startTime)) calcPosPerc = self.getPosition(angles, True) newAngles = [] for i in range(1, self.numberOfLinks - 1): newAngles.append(angles[i]) outOfRange = False for i in range(len(calcPosPerc)): if abs(calcPosPerc[i] - pos[i]) > 2: outOfRange = True break log("all Angles: {}".format(angles)) log("Out of range: {}, calcPosPerc {}".format(outOfRange, calcPosPerc), "OK") log("set pos: {}".format(pos), "OK") log("Angles are: {}".format(newAngles)) return newAngles, outOfRange
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], ) ] 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)
def fk(self, joints, full_kinematics=False): joints = np.array([0, 0, *joints, 0, 0, 0]) return Chain.forward_kinematics(self, joints, full_kinematics)
) ]) # blue # target_vector = [0.4, 0.8, .2] 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))) 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 #
mychain = Chain( name='neck', links=[ #OriginLink(), URDFLink( name="shoulder", translation_vector=[0, 0, 0.065], orientation=[-0.5 * PI, 0, 0], rotation=[0, 0, 1], ) # URDFLink( # name="elbow", # translation_vector=[0, 0, 0.129], # orientation=[-PI*(156/180.0)+theta2, 0, 0], # rotation=[0, 0, 1], # ), # URDFLink( # name="wrist", # translation_vector=[0, 0, 0.065], # orientation=[(99/180.0)*PI+theta3, 0, 0], # rotation=[0, 0, 1], # ), # URDFLink( # name="head", # translation_vector=[0, 0, 0.083], # orientation=[(61/180.0)*PI+theta4, 0, 0], # rotation=[0, 0, 1], # ) ]) print(mychain.forward_kinematics([0] * 1))
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
URDFLink( name="ankle", translation_vector=[0, 0, -1.13], orientation=[0, 0, 0], 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)
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()