<s> """<STR_LIT>""" <EOL> import math <EOL> """<STR_LIT>""" <EOL> heightFromBase = <NUM_LIT> + <NUM_LIT> <EOL> lengthRearArm = <NUM_LIT> <EOL> lengthForeArm = <NUM_LIT> <EOL> distToTool = <NUM_LIT> <EOL> armSquaredConst = pow ( lengthRearArm , <NUM_LIT:2> ) + pow ( lengthForeArm , <NUM_LIT:2> ) <EOL> armDoubledConst = <NUM_LIT> * lengthRearArm * lengthForeArm <EOL> radiansToDegrees = <NUM_LIT> / math . pi <EOL> degreesToRadians = math . pi / <NUM_LIT> <EOL> class DobotInverseKinematics : <EOL> def __init__ ( self , debug = False ) : <EOL> self . _debugOn = debug <EOL> def _debug ( self , * args ) : <EOL> if self . _debugOn : <EOL> for arg in args : <EOL> print arg , <EOL> print <EOL> def convert_cartesian_coordinate_to_arm_angles ( self , x , y , z ) : <EOL> distanceFromOriginToEndPoint = self . get_distance_from_origin_to_cartesian_point_3D ( x , y , z - heightFromBase ) <EOL> self . _debug ( '<STR_LIT>' , str ( distanceFromOriginToEndPoint ) ) <EOL> if ( distanceFromOriginToEndPoint > ( lengthRearArm + lengthForeArm ) ) : <EOL> return [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> baseAngle = self . get_polar_coordinate_angle_from_cartesian_x_y_coordinate ( x , y ) <EOL> radiusToEndPoint = self . get_polar_coordinate_radius_from_cartesian_x_y_coordinate ( x , y ) <EOL> armAngles = self . get_arm_angles_from_radius_z_coordinate_using_2d_revolute_revolute_inverse_kinematics ( radiusToEndPoint , z - heightFromBase ) <EOL> rearArmAngle = armAngles [ <NUM_LIT:0> ] <EOL> foreArmAngle = armAngles [ <NUM_LIT:1> ] <EOL> return [ baseAngle , rearArmAngle , foreArmAngle ] <EOL> def get_arm_angles_from_radius_z_coordinate_using_2d_revolute_revolute_inverse_kinematics ( self , r , z ) : <EOL> eq = ( armSquaredConst - pow ( r , <NUM_LIT:2> ) - pow ( z , <NUM_LIT:2> ) ) / armDoubledConst <EOL> foreArmAngle = math . pi - math . atan2 ( - <NUM_LIT:1> * math . sqrt ( <NUM_LIT:1> - pow ( eq , <NUM_LIT:2> ) ) , eq ) <EOL> rearArmAngle = math . atan2 ( z , r ) - math . atan2 ( ( lengthForeArm * math . sin ( foreArmAngle ) ) , ( lengthRearArm + ( lengthForeArm * math . cos ( foreArmAngle ) ) ) ) <EOL> return [ rearArmAngle , foreArmAngle ] <EOL> def get_polar_coordinate_angle_from_cartesian_x_y_coordinate ( self , x , y ) : <EOL> angle = math . atan2 ( y , x ) <EOL> return angle <EOL> def get_polar_coordinate_radius_from_cartesian_x_y_coordinate ( self , x , y ) : <EOL> radius = math . sqrt ( pow ( x , <NUM_LIT:2> ) + pow ( y , <NUM_LIT:2> ) ) <EOL> return radius <EOL> def get_distance_from_origin_to_cartesian_point_3D ( self , x , y , z ) : <EOL> distanceToEndPoint = math . sqrt ( pow ( x , <NUM_LIT:2> ) + pow ( y , <NUM_LIT:2> ) + pow ( z , <NUM_LIT:2> ) ) <EOL> return distanceToEndPoint <EOL> def check_for_angle_limits_is_valid ( self , baseAngle , rearArmAngle , foreArmAngle ) : <EOL> ret = True <EOL> if ( - <NUM_LIT:20> > rearArmAngle > <NUM_LIT> ) : <EOL> print '<STR_LIT>' <EOL> ret = False <EOL> if ( - <NUM_LIT> > foreArmAngle > <NUM_LIT> ) : <EOL> print '<STR_LIT>' <EOL> ret = False <EOL> return ret <EOL> if __name__ == '<STR_LIT:__main__>' : <EOL> ik = DobotInverseKinematics ( ) <EOL> commandFlag = True <EOL> while ( commandFlag ) : <EOL> x = input ( '<STR_LIT>' ) <EOL> y = input ( '<STR_LIT>' ) <EOL> z = input ( '<STR_LIT>' ) <EOL> angles = ik . convert_cartesian_coordinate_to_arm_angles ( float ( x ) , float ( y ) , float ( z ) ) <EOL> if ( angles [ <NUM_LIT:0> ] != - <NUM_LIT> ) : <EOL> print ( '<STR_LIT>' + str ( x ) + '<STR_LIT>' + str ( y ) + '<STR_LIT>' + str ( z ) + '<STR_LIT>' ) <EOL> print ( '<STR_LIT>' + str ( angles [ <NUM_LIT:0> ] ) ) <EOL> print ( '<STR_LIT>' + str ( angles [ <NUM_LIT:1> ] ) ) <EOL> print ( '<STR_LIT>' + str ( angles [ <NUM_LIT:2> ] ) ) <EOL> print ( '<STR_LIT:\n>' ) <EOL> else : <EOL> print ( '<STR_LIT>' ) <EOL> print ( '<STR_LIT:\n>' ) <EOL> s = input ( '<STR_LIT>' ) <EOL> if ( s == '<STR_LIT:y>' ) : <EOL> commandFlag = False <EOL> print ( '<STR_LIT:\n>' ) </s>
