Fixed edge cases when calculating base angle
This commit is contained in:
BlueOceanWave committed
1 parent
6053d1b4ec
commit
4bc3e30116
1 file changed
+188
-147
+188
-147
@@ -9,7 +9,7 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"execution_count": 10,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
@@ -28,7 +28,7 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"execution_count": 11,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
@@ -52,15 +52,14 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 75,
|
||||
"execution_count": 53,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"flip\n",
|
||||
"[62.18186071534606, -126.63734509021032, 142.6959710408549, -106.0586259506446]\n"
|
||||
"[-243.619306486927, -123.2097721836616, 140.34764917140853, -107.13787698774695, -90.0, 90.0]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -77,28 +76,29 @@
|
||||
" # Return angle\n",
|
||||
" return normalized_theta\n",
|
||||
"\n",
|
||||
"def get_joints_from_xyz_rel(x, y, z, rx=0, ry=0, rz=0, tool_offset=(0.145, 0, 0.14), initial_guess = (math.pi/2, math.pi/2, 0)):\n",
|
||||
"def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0)):\n",
|
||||
" # Get limbs and offsets\n",
|
||||
" offset_x, offset_y, offset_z = tool_offset\n",
|
||||
" l_bs, l1, l2, l3 = (0.14, .422864, .359041, .15)\n",
|
||||
"\n",
|
||||
" offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset\n",
|
||||
" l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997) # Limb lengths\n",
|
||||
" #l3=0.15\n",
|
||||
" \n",
|
||||
" # Calculate base angle and r relative to shoulder joint\n",
|
||||
" def calculate_theta(x, y, a):\n",
|
||||
" # Calculate if we need the + or - in our equations\n",
|
||||
" if (x>-a and y>=0) or (x>a and y<0):\n",
|
||||
" print('no flip')\n",
|
||||
" if (x>a and y>=0) or (x>-a and y<0):\n",
|
||||
" flip = 1\n",
|
||||
" elif (x<-a and y>=0) or (x<a and y<0):\n",
|
||||
" print('flip')\n",
|
||||
" elif (x<a and y>=0) or (x<-a and y<0):\n",
|
||||
" flip = -1\n",
|
||||
" else: \n",
|
||||
" print('critical')\n",
|
||||
" # Critical section (x=a, or x=-a). Infinite slope\n",
|
||||
" # Return 0 or 180 depending on sign\n",
|
||||
" return math.atan2(y, 0)\n",
|
||||
" return math.atan2(y, 0) - math.pi/2\n",
|
||||
" \n",
|
||||
" # Calculate tangent line y = mx + b\n",
|
||||
" m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
|
||||
" if abs(a) != abs(x): # If there is no division by 0\n",
|
||||
" m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
|
||||
" else: # Deal with edge case when x^2=a^2\n",
|
||||
" m = flip*(-a*a+y*y)/(a*y-flip*abs(a*y))\n",
|
||||
" b = flip * a * math.sqrt(1+m*m)\n",
|
||||
"\n",
|
||||
" # Calculate equivalent tangent point on circle\n",
|
||||
@@ -108,11 +108,11 @@
|
||||
" # Calculate base angle, make angle negative if flip=1\n",
|
||||
" theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)\n",
|
||||
"\n",
|
||||
" return theta\n",
|
||||
" return theta \n",
|
||||
" \n",
|
||||
" base_theta = calculate_theta(x, y, l_bs)\n",
|
||||
" cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(l_bs)\n",
|
||||
" r = math.sqrt((x-cx)**2 + (y-cy)**2) - 0.1\n",
|
||||
" cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)\n",
|
||||
" r = math.sqrt((x-cx)**2 + (y-cy)**2) \n",
|
||||
"\n",
|
||||
"\n",
|
||||
" # Formulas to find out joint positions for (r, z)\n",
|
||||
@@ -120,7 +120,7 @@
|
||||
" a, b, c = p \n",
|
||||
"\n",
|
||||
" return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r\n",
|
||||
" l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (z+offset_z), # z\n",
|
||||
" l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z), # z\n",
|
||||
" a-b-c) # wrist angle\n",
|
||||
"\n",
|
||||
"\n",
|
||||
@@ -130,13 +130,13 @@
|
||||
" # Return result\n",
|
||||
" return base, shoulder, elbow, wrist1, ry, rz\n",
|
||||
"\n",
|
||||
"def get_joints_from_xyz_abs(x, y, z):\n",
|
||||
" joints = get_joints_from_xyz_rel(x, y, z)\n",
|
||||
"def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):\n",
|
||||
" joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)\n",
|
||||
"\n",
|
||||
" # Joint offsets\n",
|
||||
" # Base, Shoulder, Elbow, Wrist\n",
|
||||
" inverse = [1, -1, 1, 1, 1, 1]\n",
|
||||
" offsets = [-math.pi/2, 0, 0, -math.pi/2, -math.pi/2, 0]\n",
|
||||
" offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]\n",
|
||||
"\n",
|
||||
" # Return adjusted joint positions\n",
|
||||
" return [o+j*i for j, o, i in zip(joints, offsets, inverse)]\n",
|
||||
@@ -147,23 +147,24 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 64,
|
||||
"execution_count": 58,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"no flip\n"
|
||||
"flip\n",
|
||||
"m: -0.9031146536634159\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"-126.86989764584402"
|
||||
"47.914345304219715"
|
||||
]
|
||||
},
|
||||
"execution_count": 64,
|
||||
"execution_count": 58,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
@@ -181,10 +182,15 @@
|
||||
" print('critical')\n",
|
||||
" # Critical section (x=a, or x=-a). Infinite slope\n",
|
||||
" # Return 0 or 180 depending on sign\n",
|
||||
" return math.atan2(y, 0)\n",
|
||||
" return math.degrees(math.atan2(y, 0)-math.pi/2)\n",
|
||||
" \n",
|
||||
" # Calculate tangent line y = mx + b\n",
|
||||
" m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
|
||||
" if abs(a) != abs(x): # If there is no division by 0\n",
|
||||
" m = (x*y + math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)\n",
|
||||
" else: # Deal with edge case when x^2=a^2\n",
|
||||
" m = flip*(-a*a+y*y)/(a*y-flip*abs(a*y))\n",
|
||||
" \n",
|
||||
" print('m:',m)\n",
|
||||
" b = flip * a * math.sqrt(1+m*m)\n",
|
||||
"\n",
|
||||
" # Calculate equivalent tangent point on circle\n",
|
||||
@@ -194,9 +200,9 @@
|
||||
" # Calculate base angle\n",
|
||||
" theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)\n",
|
||||
"\n",
|
||||
" return theta\n",
|
||||
" return math.degrees(theta)\n",
|
||||
"\n",
|
||||
"calculate_theta(5,-5,1)"
|
||||
"calculate_theta(-0.1333,0.3,0.1333)"
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -208,26 +214,25 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 70,
|
||||
"execution_count": 30,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"no flip\n",
|
||||
"Target position (x,y,z): 0.3 0.3 0.3\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [-25.7322, 113.4225, 127.6133, -14.1908]\n",
|
||||
"no flip\n",
|
||||
"Robot Angles: [-115.7322, -113.4225, 127.6133, -104.1908]\n",
|
||||
"elbow (x,y): -0.168 0.388\n",
|
||||
"wrist (x,y): 0.18 0.3\n",
|
||||
"tool (x,y): 0.33 0.3\n"
|
||||
"R: 0.4243\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [-26.688, 105.6932, 101.1834, 4.5098]\n",
|
||||
"Robot Angles: [-116.688, -105.6932, 101.1834, -85.4902, -90.0, 90.0]\n",
|
||||
"elbow (x,y): -0.114 0.407\n",
|
||||
"wrist (x,y): 0.244 0.435\n",
|
||||
"tool (x,y): 0.394 0.435\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
@@ -243,10 +248,11 @@
|
||||
" l1, l2, l3 = (.422864, .359041, .15)\n",
|
||||
" offset_x, offset_y, offset_z = (0.145, 0, 0.195)\n",
|
||||
" r, theta = cartesian_to_polar(x, y)\n",
|
||||
" base, shoulder, elbow, wrist = get_joints_from_xyz_rel(x, y, z)\n",
|
||||
" base, shoulder, elbow, wrist, _, _ = get_joints_from_xyz_rel(x, y, z)\n",
|
||||
"\n",
|
||||
" # Print angles\n",
|
||||
" print('Target position (x,y,z):', x, y, z)\n",
|
||||
" print('R: ', round(math.sqrt(x**2+y**2),4))\n",
|
||||
" print('Angles (base, shoulder, elbow, wrist):', [round(math.degrees(i), 4) for i in [base, shoulder, elbow, wrist]])\n",
|
||||
" print('Robot Angles:', [round(math.degrees(i), 4) for i in get_joints_from_xyz_abs(x, y, z)])\n",
|
||||
"\n",
|
||||
@@ -287,7 +293,7 @@
|
||||
"\n",
|
||||
" # Display z arrow\n",
|
||||
" plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(tx, ty), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
||||
" plt.annotate(f'z={round(ty,4)}', xy=(x3+0.01, y2/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
|
||||
" plt.annotate(f'z={round(ty,4)}', xy=(tx+0.01, ty/2), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
|
||||
" \n",
|
||||
" # Display plot\n",
|
||||
" ax = plt.subplot(111)\n",
|
||||
@@ -300,85 +306,9 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 18,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Target position (x,y,z): 0.3 0.3 0.3\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [33.9862, 85.2162, 73.3858, 11.8304]\n",
|
||||
"Robot Angles: [33.9862, -85.2162, 73.3858, -78.1696]\n",
|
||||
"elbow (x,y): 0.035 0.421\n",
|
||||
"wrist (x,y): 0.387 0.495\n",
|
||||
"tool (x,y): 0.537 0.495\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Target position (x,y,z): -0.3 -0.3 0.7\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [-117.3239, 78.0693, 0.0255, 79.0945]\n",
|
||||
"Robot Angles: [-117.3239, -78.0693, 0.0255, -10.9055]\n",
|
||||
"elbow (x,y): 0.087 0.414\n",
|
||||
"wrist (x,y): 0.162 0.765\n",
|
||||
"tool (x,y): 0.312 0.762\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"name": "stderr",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"c:\\Users\\nadee\\AppData\\Local\\Programs\\Python\\Python310\\lib\\site-packages\\scipy\\optimize\\_minpack_py.py:178: RuntimeWarning: The iteration is not making good progress, as measured by the \n",
|
||||
" improvement from the last ten iterations.\n",
|
||||
" warnings.warn(msg, RuntimeWarning)\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Target position (x,y,z): -0.3 0.4 0.2\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [111.1813, 100.8341, 104.0795, -3.2455]\n",
|
||||
"Robot Angles: [111.1813, -100.8341, 104.0795, -93.2455]\n",
|
||||
"elbow (x,y): -0.079 0.415\n",
|
||||
"wrist (x,y): 0.279 0.395\n",
|
||||
"tool (x,y): 0.429 0.395\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"draw_arm(0.3, 0.3, 0.3)\n",
|
||||
"draw_arm(-0.3, -0.3, 0.7)\n",
|
||||
@@ -394,34 +324,9 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 19,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"application/vnd.jupyter.widget-view+json": {
|
||||
"model_id": "30dd696bcdc14aec9a1d4c17fb4787d4",
|
||||
"version_major": 2,
|
||||
"version_minor": 0
|
||||
},
|
||||
"text/plain": [
|
||||
"interactive(children=(FloatSlider(value=0.3, description='x', max=1.0, step=0.01), FloatSlider(value=0.3, desc…"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"<function __main__.draw_arm(x, y, z)>"
|
||||
]
|
||||
},
|
||||
"execution_count": 19,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from ipywidgets import interact, FloatSlider\n",
|
||||
"\n",
|
||||
@@ -431,6 +336,142 @@
|
||||
" z=FloatSlider(min=0, max=1, step=0.01, value=0.3))\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 66,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Angles: [153.31198894476785, 105.69315886537287, 101.18338602047919, 4.509772844893658, -90.0, 0.0]\n",
|
||||
"(cx, cy): (-0.11909893611698993, 0.0598693027836566)\n",
|
||||
"-0.06746836046451549 0.16257881075200548\n",
|
||||
"-0.24309496877200426 -0.18679793543865197\n",
|
||||
"-0.29999999999998095 -0.29999999999996196\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Target position (x,y,z): -0.3 -0.3 0.3\n",
|
||||
"R: 0.4243\n",
|
||||
"Angles (base, shoulder, elbow, wrist): [153.312, 105.6932, 101.1834, 4.5098]\n",
|
||||
"Robot Angles: [63.312, -105.6932, 101.1834, -85.4902, -90.0, 90.0]\n",
|
||||
"elbow (x,y): -0.114 0.407\n",
|
||||
"wrist (x,y): 0.244 0.435\n",
|
||||
"tool (x,y): 0.394 0.435\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "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 truncated
|
||||
"text/plain": [
|
||||
"<Figure size 640x480 with 1 Axes>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"\n",
|
||||
"def display_arm(x, y, z):\n",
|
||||
" # Get joint and position information\n",
|
||||
" angles = get_joints_from_xyz_rel(x, y, z)\n",
|
||||
" print('Angles:', [math.degrees(angle) for angle in angles])\n",
|
||||
"\n",
|
||||
" offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset\n",
|
||||
" l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997) # Limb lengths\n",
|
||||
" cx, cy = l_bs*math.cos(angles[0]), l_bs*math.sin(angles[0]) # Base tangent point\n",
|
||||
" line_angle = math.pi/2+angles[0]\n",
|
||||
" print(f'(cx, cy): ({cx}, {cy})')\n",
|
||||
"\n",
|
||||
"\n",
|
||||
" # Plot coordinate system\n",
|
||||
" fig = plt.figure()\n",
|
||||
" ax = fig.add_subplot(1, 1, 1)\n",
|
||||
" ax.spines['left'].set_position('center')\n",
|
||||
" ax.spines['bottom'].set_position('center')\n",
|
||||
" ax.spines['right'].set_color('none')\n",
|
||||
" ax.spines['top'].set_color('none')\n",
|
||||
" ax.set_yticklabels([])\n",
|
||||
" ax.set_xticklabels([])\n",
|
||||
" ax.set_xticks([])\n",
|
||||
" ax.set_yticks([])\n",
|
||||
"\n",
|
||||
" # Target point\n",
|
||||
" # plt.plot(x, y, 'go')\n",
|
||||
"\n",
|
||||
" # Circle\n",
|
||||
" circle = plt.Circle((0, 0), l_bs, color='b', fill=False, linewidth=1)\n",
|
||||
" plt.plot([0, cx], [0, cy], color='b', linewidth=1)\n",
|
||||
" ax.add_patch(circle)\n",
|
||||
"\n",
|
||||
" # Draw limbs\n",
|
||||
" # Shoulder\n",
|
||||
" x1, y1 = polar_to_cartesian(l1*math.cos(angles[1]), line_angle)\n",
|
||||
" x1, y1 = cx+x1, cy+y1\n",
|
||||
" print(x1, y1)\n",
|
||||
" plt.plot([cx, x1], [cy, y1], color='cyan', linewidth=3)\n",
|
||||
"\n",
|
||||
" # Elbow\n",
|
||||
" x2, y2 = polar_to_cartesian(l2*math.cos(angles[1]-angles[2]), line_angle)\n",
|
||||
" x2 += x1\n",
|
||||
" y2 += y1\n",
|
||||
" print(x2, y2)\n",
|
||||
" plt.plot([x1, x2], [y1, y2], color='orange', linewidth=2)\n",
|
||||
"\n",
|
||||
" # Wrist\n",
|
||||
" x3, y3 = polar_to_cartesian(l3*math.cos(angles[1]-angles[2]-angles[3]), line_angle)\n",
|
||||
" x3 += x2\n",
|
||||
" y3 += y2 \n",
|
||||
" print(x3, y3)\n",
|
||||
" plt.plot([x2, x3], [y2, y3], color='red', linewidth=2)\n",
|
||||
"\n",
|
||||
" # Display angle\n",
|
||||
" plt.text(0.01, -0.01-cy/abs(cy+0.00001)*0.02, f'{round(math.degrees(angles[0]), 2)}°', fontsize=7)\n",
|
||||
"\n",
|
||||
" # Display x arrow\n",
|
||||
" sign = x3/abs(x3)\n",
|
||||
" plt.annotate(f'', xy=(0, y3), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
||||
" plt.annotate(f'x={round(x3,3)}', xy=(-0.1-sign*0.13, y3-0.015), xycoords='data', xytext=(x2/2, 0), textcoords='offset points')\n",
|
||||
"\n",
|
||||
" # Display y arrow\n",
|
||||
" sign = y3/abs(y3)\n",
|
||||
" plt.annotate(f'', xy=(x3, 0), xycoords='data', xytext=(x3, y3), textcoords='data', arrowprops={'arrowstyle': '<->'})\n",
|
||||
" plt.annotate(f'y={round(y3,3)}', xy=((x3-0.1), -0.015-sign*0.03), xycoords='data', xytext=(x3/2, 0), textcoords='offset points')\n",
|
||||
"\n",
|
||||
"\n",
|
||||
" # Set axis limits and labels\n",
|
||||
" axis_limit = math.hypot(x, y)+.1\n",
|
||||
" plt.axis('square')\n",
|
||||
" plt.xlim(-axis_limit, axis_limit)\n",
|
||||
" plt.ylim(-axis_limit, axis_limit)\n",
|
||||
"\n",
|
||||
" # Adjust the position of axis labels\n",
|
||||
" plt.xlabel('+x', horizontalalignment='right', x=1.05)\n",
|
||||
" plt.ylabel('+y', verticalalignment='top', rotation=0, y=1.05)\n",
|
||||
" plt.show()\n",
|
||||
"\n",
|
||||
" draw_arm(x,y,z)\n",
|
||||
"\n",
|
||||
"display_arm(-0.3,-0.3,0.3)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
|
||||
Reference in new issue
Block a user