From 4a7ddf51299e13d72dd549864b26cadc811ef44d Mon Sep 17 00:00:00 2001
From: Ioana Anamaria <ioana.ulici@student.utcluj.ro>
Date: Tue, 1 Nov 2022 14:38:00 +0200
Subject: [PATCH] Updating the exercise for the AL5D_mdw, changing the
collision bound
---
al5d_control.py | 2 +-
lab05_IKM.ipynb | 153 ++++++++++++++++++------------------------------
2 files changed, 59 insertions(+), 96 deletions(-)
diff --git a/al5d_control.py b/al5d_control.py
index 0f44c5c..29d69f2 100644
--- a/al5d_control.py
+++ b/al5d_control.py
@@ -41,7 +41,7 @@ class AL5DControl:
grip = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')
tool = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')
- if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):
+ if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < -0.01):
return True
else:
return False
diff --git a/lab05_IKM.ipynb b/lab05_IKM.ipynb
index 90de3e2..78266ec 100644
--- a/lab05_IKM.ipynb
+++ b/lab05_IKM.ipynb
@@ -8,13 +8,8 @@
"## Determination of an inverse geometric model\n",
"\n",
"In the previous works we analysed the problem of calculating the position and orientation of a Cartesian system attached on the end-effector, when the robot coordinates of each joint of the robot is known. This exercise proposes \n",
- "an approach a bit more difficult: we want to determine the joint coordinates for a specific position of the end-effector. In other words, determine a set of coordinates of the robot $q_1,q_2,...,q_n$, that ensure a specific position and orientation of the end-effector. This problem is known as the __inverse geometric model__ of a kinematic chain. The main objective of this exercise is the description of a heuristic method for determining this inverse geometric model of a robotic arm and to apply this method in two robotic kinematic structures."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "an approach a bit more difficult: we want to determine the joint coordinates for a specific position of the end-effector. In other words, determine a set of coordinates of the robot $q_1,q_2,...,q_n$, that ensure a specific position and orientation of the end-effector. This problem is known as the __inverse geometric model__ of a kinematic chain. The main objective of this exercise is the description of a heuristic method for determining this inverse geometric model of a robotic arm and to apply this method in two robotic kinematic structures.\n",
+ "\n",
"### 5.1 Theoretical considerations\n",
"\n",
"The problem of determining the inverse geometric model of a robot is a non-linear problem. For a robotic structure, we are starting with the numerical values of the matrix that describes the direct geometric model ($^0T_N$) and we aim at determining the set of joint coordinates of the robot ($q_1,q_2,...q_n$). For a robot with 6 degrees of freedom (6 motor joints), we obtain a system with 12 equations and 6 unknowns. However, from the 9 equations that result from the correspondence of the orientation from matrix $^T_6$, only three of them are independent and the other 6 are redundant. This limits us to the determination of maximum 3 independent variables. If, on these three equations, we add the three equations resulting from the correspondence of the position vectors from matrix $^T_6$, we obtain a set of 6 independent equations that allow us to determine a maximum of 6 independent variables. These variables are corresponding to the 6 degrees of freedom of the robot. This system of equations is non-linear and transcendent, often proving difficult to solve. Like all non-linear systems of equations, the main problems are raised by the existence of solutions, existence of multiple solutions and the method to use for solving the system.\n",
@@ -34,14 +29,9 @@
" </figure>\n",
"</center>\n",
"\n",
- "The choice of one over the other solution depends on external restrictions and on the construction elements of the robot. However, once a solution has been chosen, it should be used with continued consistency to avoid unnecessary displacement of the robot between the two configurations."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### *Heuristic method for determining the IGM*\n",
+ "The choice of one over the other solution depends on external restrictions and on the construction elements of the robot. However, once a solution has been chosen, it should be used with continued consistency to avoid unnecessary displacement of the robot between the two configurations.\n",
+ "\n",
+ "### 5.2 Heuristic method for determining the IGM\n",
"\n",
"__I__. We equate the homogeneous transformation matrices of the manipulator (direct geometric model) with the general homogeneous transformation matrix (\\ref{eq4.6}). If we are seeking a specific position, the homogeneous transformation matrix of the manipulator is equated with a matrix that describes this specific position.\n",
"\n",
@@ -119,20 +109,10 @@
" { - s_2 } & 0 & {c_2 } & {l_1 - l_2\\cdot s_2 } \\\\\n",
" 0 & 0 & 0 & 1 \\\\\n",
"\\end{array} }} \\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "The determination of the inverse geometric model, assumes the equation of the direct transformation matrix ($T$) with the general matrix of a homogeneous transformation ($T_g$) (see also relation eq.1.12):"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "\\end{equation}\n",
+ "\n",
+ "The determination of the inverse geometric model, assumes the equation of the direct transformation matrix ($T$) with the general matrix of a homogeneous transformation ($T_g$) (see also relation eq.1.12):\n",
+ "\n",
"\n",
"\\begin{equation}\\label{eq4.6}\n",
"T = T_{g}\n",
@@ -151,13 +131,8 @@
" {X_Z } & {Y_Z } & {Z_Z } & {P_Z } \\\\\n",
" 0 & 0 & 0 & 1 \\\\\n",
"\\end{array} \\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "\\end{equation}\n",
+ "\n",
"\n",
"For the determination of the joint coordinates vector of the robot $q=[q_1,\\; q_2]^T$ we construct a system of equations:\n",
"\\begin{equation}\\label{mgi-eq3}\n",
@@ -170,24 +145,14 @@
"\n",
"\\begin{equation}\n",
"\\frac{P_Y }{P_X } = \\frac{s_1 }{c_1 } \\;\\Longrightarrow \\;q_1 = \\arctan\\left({\\frac{P_Y }{P_X }} \\right)\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "\\end{equation}\n",
+ "\n",
"For the determination of $q_2$ we resort to the pre-multiplication of both parts of the matrix equation (\\ref{eq4.7}) with the inverse of the transformation matrix of the first kinematic joint (step __VI__, obtaining a new set of equations:\n",
"\n",
"\\begin{equation}\n",
"T_1^{ - 1} \\cdot T_1 \\cdot T_2 \\cdot T_3 \\cdot T_4= T_1^{ - 1} \\cdot T_g\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "\\end{equation}\n",
+ "\n",
"\n",
"\\begin{equation}\n",
"\\left[ {{\\begin{array}{*{20}c}\n",
@@ -204,13 +169,8 @@
" {X_Z } & {Y_Z } & {Z_Z } & {P_Z } \\\\\n",
" 0 & 0 & 0 & 1 \\\\\n",
"\\end{array} }} \\right]\n",
- "\\end{equation}\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
+ "\\end{equation}\n",
+ "\n",
"resulting in the system\n",
"\n",
"\\begin{equation}\n",
@@ -223,29 +183,22 @@
"\\begin{equation}\n",
"\\frac{l_1 - P_Z }{P_X\\cdot c_1 + P_Y\\cdot s_1 } = \\frac{s_2 }{c_2 } \\;\\Longrightarrow\\;\n",
" q_2 = \\arctan\\left( {\\frac{l_1 - P_Z }{P_X \\cdot c_1 + P_Y\\cdot s_1 }} \\right)\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "# 5.2. Robotic toolbox\n",
+ "\\end{equation}\n",
+ "\n",
+ "### 5.3. Robotics toolbox\n",
"In the previous laboratory, we saw how to define links and how to combine them\n",
"into a robotic structure, using the __Revolute__, __Prismatic__ and __DHRobot__ methods. We also saw how to calculate the end effector pose using the __fkine__, the jacobian of the robot using the __jacob0__, and how to visualise the robot using the __plot__ methods.\n",
"\n",
"In the examples above, it is 'easy' to calculate the inverse kinematics models by hand, but for more complex robots, we need to solve it numerically. The robotic toolbox has methods for solving the inverse kinematics model, using numerical methods.\n",
"The toolbox can do this using the __ikine__ method (from inverse kinematics). The method works by providing a desired end-effector pose (position and orientation) and gives back the joint coordinates the result in the desired pose. As we know, the inverse kinematics model might have more than just one solution for a specific pose. The way numerical methods work, they start looking for a solution around a initial guess (which we can provide), and slowly converge to the joint coordinates that result in the desired pose. The convergence point might depend on the initial guess we are providing.\n",
"\n",
- "## Usage\n",
+ "\n",
"\n",
"The __ikine_LM__ method provides a vector of joint coordinates that results in the end-effector pose to be the one desired. To do so, we need to provide a pose as an input to the method. The pose is provided in the form of a 4x4 homogeneous transformation matrix. This transformation matrix provides information about the 6 degrees of freedom available (3 positions and 3 orientations).\n",
"\n",
"The __ikine_LM__ method can solve the inverse kinematics problem even for kinematic chains with less than 6 degrees of freedom. In that case, we need to specify for which DoFs we want the inverse kinematics problem solved. We do this by providing a __mask__ as an input, which is a vector with six logical elements (0 or 1), specifying with 1 those DoFs that we want to solve the inverse kinematics for. The number of DoFs that we can solve the inverse kinematics for __must__ be equal to the DoFs of the robot itself.\n",
"\n",
- "## Example\n",
+ "### 5.4. Example\n",
"\n",
"Below, we see an example of how the __ikine_LM__ method works. \n",
"\n",
@@ -327,9 +280,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
- "# 5.3. Proposed problems\n",
+ "### 5.5. Proposed problems\n",
"\n",
- "## Analytical inverse kinematics\n",
+ "#### 1.Analytical inverse kinematics\n",
"We consider the robotic structure with 3 degrees of freedom from figure fig 5.3, for which $l_1=0.5\\;m$.\n",
"\n",
"<center>\n",
@@ -341,24 +294,21 @@
"\n",
"* Determine the direct geometric model.\n",
"* Determine the inverse geometric model for X, Y, and Z position of the end-effector\n",
- "* Calculate the joint coordinates that result in the following position for the end-effector: [2,4,0.5]"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Numerical inverse kinematics\n",
+ "* Calculate the joint coordinates that result in the following position for the end-effector: [2,4,0.5]\n",
+ "\n",
+ "#### 2.Numerical inverse kinematics\n",
+ "\n",
+ "* Check what values does the gripper need for open/close positions\n",
"\n",
"Using the AL5D robot and the robotic toolbox, implement a program that:\n",
- "* Picks a box from coordinates X=18.6cm, Y=12.1cm, Z=3.7cm, and places it at coordinates X=20.88cm, Y=-9.45cm, and Z=3.7cm.\n",
- "* Consider a rotation around Z by 33 degrees, a rotation around Y by 54 degrees and a rotation around X by 90 degrees for the picking pose.\n",
- "* Consider a rotation around Z by -25 degrees, a rotation around Y by 70 degrees and a rotation around X by 90 degrees for the placing pose.\n",
+ "* Picks a box from coordinates X=13.5cm, Y=-8.7cm, Z=11cm, and places it at coordinates X=12.2cm, Y=7cm, and Z=11cm.\n",
+ "* Consider a rotation around Y by -15 degrees and a rotation around X by -30 degrees for the picking pose.\n",
+ "* Consider a rotation around Y by -15 degrees and a rotation around X by 30 degrees for the placing pose.\n",
"* In both cases, the rotations are relative and are being multiplied from the right (Transl*RotZ*RotY*RotX).\n",
"* Keep in mind that the box is lying flat on the table.\n",
- "* Use the ikine_LM method of the robopy module for solving the inverse kinematics for the specific poses.\n",
+ "* Use the ikine_LM method of the robotics toolbox for solving the inverse kinematics for the specific poses.\n",
"* Specify an initial guess for any used poses.\n",
- "* Specify that you are interested in the position only when solving the inverse kinematics.\n",
+ "* Specify that you are interested in the position and in the Y, Z orientations when solving the inverse kinematics.\n",
"* Then use the provided functions for controlling the joint positions so that the desired poses are reached. Keep in mind to keep a delay between any two poses."
]
},
@@ -368,23 +318,36 @@
"metadata": {},
"outputs": [],
"source": [
- "# Importing necessary modules\n",
- "from al5d_control import *\n",
+ "%matplotlib nbagg\n",
+ "from spatialmath import *\n",
+ "from spatialmath.base import *\n",
+ "from math import *\n",
"\n",
- "rrob = AL5DControl()\n",
+ "# Import AL5D model\n",
+ "rob = rtb.models.DH.AL5D_mdw()\n",
"\n",
- "# Solve the ikine for the respective poses\n",
- "# The solution will be in radians\n",
+ "# simulation of robot in pose q\n",
+ "q = np.array([0,0,0,0,0,0])\n",
+ "rob.plot(q)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from al5d_control import *\n",
+ "from time import sleep \n",
"\n",
- "# Control the robot to go to each pose using the robot_control function\n",
- "# Give radians as input for the joint values\n",
+ "# for sending the commands\n",
+ "rrob = AL5DControl()\n",
"\n",
"# example of using the robot_control\n",
- "# !!! USE THESE FUNCTIONS WITH YOUR OWN SOLUTION TO START THE ROBOT ONLY AFTER SIMULATION\n",
+ "# !!! USE THESE FUNCTIONS WITH YOUR OWN SOLUTION TO START THE ROBOT ONLY AFTER CHECKING IN SIMULATION\n",
"rrob.robot_control(0,0,0,0,0,0)\n",
- "time.sleep(4)\n",
- "#rrob.robot_control(0,pi/2,pi/2,pi/2,0,3*pi/2)\n",
- "rrob.robot_control(30,-20,0,0,45,-45)\n"
+ "sleep(4)\n",
+ "rrob.robot_control(10,0,0,0,0,0)"
]
}
],
--
GitLab