diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8709cc7 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +*.pyc +*build/ +*dist/ +*.egg-info* diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..9fa7550 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 NxRLab + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md index efd1210..f2ad38c 100644 --- a/README.md +++ b/README.md @@ -1,92 +1,29 @@ -# Modern Robotics +# Modern Robotics: Mechanics, Planning, and Control +# Code Library +This repository contains the code library accompanying [_Modern Robotics: +Mechanics, Planning, and Control_](http://modernrobotics.org) (Kevin Lynch +and Frank Park, Cambridge University Press 2017). The +[user manual](/doc/MRlib.pdf) is in the doc directory. +The functions are available in: -## Getting started +* Python +* MATLAB +* Mathematica -To make it easy for you to get started with GitLab, here's a list of recommended next steps. +Each function has a commented section above it explaining the inputs required for its use as well as an example of how it can be used and what the output will be. This repository also contains a pdf document that provides an overview of the available functions using MATLAB syntax. Functions are organized according to the chapter in which they are introduced in the book. Basic functions, such as functions to calculate the magnitude of a vector, normalize a vector, test if the value is near zero, and perform matrix operations such as multiplication and inverses, are not documented here. -Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)! +The primary purpose of the provided software is to be easy to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness. -## Add your files +Some unofficial versions in other languages are being developed: +* [C++ version](https://github.com/Le0nX/ModernRoboticsCpp) +* [Julia version](https://github.com/ferrolho/ModernRoboticsBook.jl) +* [Nim version](https://github.com/Nimbotics/ModernRoboticsNim) -- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files -- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command: +Some libraries built on ours: +* [KinematicsFromDescriptionTool](https://github.com/Interbotix/kinematics_from_description), which calculates the kinematics input parameters from a robot's URDF or robot_description parameter using ROS and Python3. +* [mr_urdf_loader](https://github.com/tjdalsckd/mr_urdf_loader), which generates `M`, `Slist`, `Blist`, `Mlist` and `Glist` parameters for kinematics and dynamics. It also provides UR5 simulation using `PyBullet`. +* [tf_rbdl](https://github.com/junhyeokahn/tf_rbdl#tf_rbdl), which refactors the Python version using the package `tensorflow`. -``` -cd existing_repo -git remote add origin http://gitlab/robotics/modern-robotics.git -git branch -M main -git push -uf origin main -``` - -## Integrate with your tools - -- [ ] [Set up project integrations](http://gitlab/robotics/modern-robotics/-/settings/integrations) - -## Collaborate with your team - -- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/) -- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html) -- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically) -- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/) -- [ ] [Automatically merge when pipeline succeeds](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html) - -## Test and Deploy - -Use the built-in continuous integration in GitLab. - -- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html) -- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing(SAST)](https://docs.gitlab.com/ee/user/application_security/sast/) -- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html) -- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/) -- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html) - -*** - -# Editing this README - -When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thank you to [makeareadme.com](https://www.makeareadme.com/) for this template. - -## Suggestions for a good README -Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information. - -## Name -Choose a self-explaining name for your project. - -## Description -Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors. - -## Badges -On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge. - -## Visuals -Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method. - -## Installation -Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection. - -## Usage -Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README. - -## Support -Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc. - -## Roadmap -If you have ideas for releases in the future, it is a good idea to list them in the README. - -## Contributing -State if you are open to contributions and what your requirements are for accepting them. - -For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self. - -You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser. - -## Authors and acknowledgment -Show your appreciation to those who have contributed to the project. - -## License -For open source projects, say how it is licensed. - -## Project status -If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers. +Any contribution is welcomed but the maintenance team for this library here doesn't vouch for the reliability of those projects. diff --git a/doc/MRlib.pdf b/doc/MRlib.pdf new file mode 100644 index 0000000..6c81709 Binary files /dev/null and b/doc/MRlib.pdf differ diff --git a/doc/MRlib.tex b/doc/MRlib.tex new file mode 100644 index 0000000..1fc8b8c --- /dev/null +++ b/doc/MRlib.tex @@ -0,0 +1,678 @@ +\documentclass[11pt]{article} + +\usepackage{amsfonts} +\usepackage{amsmath} +\usepackage{framed} +\usepackage{xcolor} +\usepackage{enumitem} +\usepackage{booktabs} +\usepackage{ltablex} +\usepackage{ifthen} +\usepackage[linktoc=all,colorlinks=true,urlcolor=blue]{hyperref} +\hypersetup{ + colorlinks, + linkcolor=blue +} + +\newcommand{\real}{\mathbb{R}} +\newcommand{\trans}{{\rm T}} + +\newenvironment{function}[4] %A new environment for functions +{ +\begin{shaded*} +\noindent +\verb~#1~ %The function's name. +\end{shaded*} +\vspace*{-5pt} +\ifthenelse{\equal{\detokenize{#2}}{\detokenize{}}} +{} % what to do if there is no description +{\noindent +Input: +\begin{description}[leftmargin=1.5cm,labelindent=0.5cm,itemsep=-4pt,topsep=0pt] +#2 +\end{description}} +\ifthenelse{\equal{\detokenize{#3}}{\detokenize{}}} +{} % what to do if there is no description +{\noindent +Output: +\begin{description}[leftmargin=1.5cm,labelindent=0.5cm,itemsep=-4pt,topsep=0pt] %Output list. +#3 +\end{description}} +\ifthenelse{\equal{\detokenize{#4}}{\detokenize{}}} +{} % what to do if there is no description +{\vspace*{5pt} \noindent #4} % otherwise give the description +\vspace*{10pt} +}{} + + +\setlength{\oddsidemargin}{0cm} %Page settings. +\setlength{\evensidemargin}{3cm} +\setlength{\marginparsep}{0.75cm} +\setlength{\marginparwidth}{2.5cm} +\setlength{\marginparpush}{1.0cm} +\setlength{\textwidth}{16.5cm} +\setlength{\textheight}{22.86cm} +\setlength{\topmargin}{-1.27cm} + +\definecolor{shadecolor}{rgb}{0.9,0.9,0.9} + +\begin{document} + +\begin{center} %Title. +\Large +{\bf Modern Robotics: Mechanics, Planning, and Control} \\ +{\bf Code Library} \\ +\normalsize +Version 1.0.1\\ +\vspace*{0.2in} +Huan Weng and Kevin M. Lynch \\ +July 6, 2018 \\ +(beta version: January 14, 2017) +\end{center} + +\section*{Introduction} + +This is the documentation for the code library accompanying \emph{Modern Robotics: Mechanics, Planning, and Control}, by Kevin M. Lynch and Frank C. Park, Cambridge University Press, 2017, \url{http://modernrobotics.org}. The code is written for MATLAB, Mathematica, and Python, and originates from students' solutions to programming assignments in courses using material from the book. The current version of the code is largely the work of Huan Weng, based on contributions from Bill Hunt, Jarvis Schultz, Mikhail Todes, Matthew Collins, Mojtaba Mozaffar, Chang Liu, and Wentao Chen. + +The code is commented and mostly self-explanatory in conjunction with the book. An example use is provided with each function. The primary purpose of the software is to be easy to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness (it does not perform error-checking on its inputs). This is to keep the code as simple and unintimidating as possible. Users are encouraged to use and modify the code however they wish; the process of using and modifying the code certainly aids in understanding the concepts in the book. + +Information on installing and using the library is available at the code website, \url{https://github.com/NxRLab/ModernRobotics}. Your feedback on bugs or documentation errors are appreciated via the issue tracker at the same site. + +This document provides an overview of the available functions using MATLAB syntax. Functions are organized according to the relevant chapter in the book. Basic functions, such as functions to calculate the magnitude of a vector, normalize a vector, test if a value is near zero, and perform matrix operations such as multiplication and inverses, are not documented here. + +Notation that is used throughout this document is summarized below. + +\begin{tabularx}{\linewidth}{llp{4.5in}} +\toprule +{\bf Math} & {\bf Computer} & \\ +{\bf symbol} & {\bf variable} & {\bf Description} \\ +\midrule +$R$ & {\tt R} & $3 \times 3$ rotation matrix in $SO(3)$. \\ +$\omega$ & {\tt omg} & $3$-vector angular velocity. \\ +$\hat{\omega}$ & {\tt omghat} & $3$-vector unit rotation axis or unit angular velocity. \\ +$\theta$ & {\tt theta} & Angle of rotation about an axis or distance traveled along a screw axis. \\ +$\hat{\omega} \theta$ & {\tt expc3} & $3$-vector of exponential coordinates for rotation. \\ +$[\omega], [\hat{\omega} \theta] $ & {\tt so3mat} & $3 \times 3$ skew-symmetric $so(3)$ representation of $\omega$ or $\hat{\omega} \theta$. \\ +$p$ & {\tt p} & $3$-vector for a position in space. \\ +$T$ & {\tt T} & $4 \times 4$ transformation matrix in $SE(3)$ corresponding to $(R,p)$. \\ +$[\operatorname{Ad}_T]$ & {\tt AdT} & $6 \times 6$ matrix adjoint representation of $T \in SE(3)$. \\ +$v$ & {\tt v} & $3$-vector linear velocity. \\ +$\mathcal{V}$ & {\tt V} & $6$-vector twist $(\omega, v)$. \\ +$\mathcal{S}$ & {\tt S} & A normalized $6$-vector screw axis $(\omega,v)$, where (a) $\|\omega\| = 1$ or (b) $\|\omega\| = 0$ and $\|v\| = 1$. \\ +$\mathcal{S}\theta$ & {\tt expc6} & $6$-vector of exponential coordinates for rigid-body motion. \\ +$[\mathcal{V}], [\mathcal{S} \theta]$ & {\tt se3mat} & $4 \times 4$ $se(3)$ representation of $\mathcal{V}$ or $\mathcal{S} \theta$. \\ +$M$ & {\tt M} & End-effector configuration in $SE(3)$ when manipulator is at its zero position. \\ +$\mathcal{B}_i$ & {\tt Blist} & $\mathcal{B}_i$ is the screw axis of the $i$th joint expressed in the end-effector frame when the manipulator is at the zero position. {\tt Blist} is a list of all the joint screw axes for the manipulator, $i = 1, \ldots, n$. \\ +$\mathcal{S}_i$ & {\tt Slist} & $\mathcal{S}_i$ is the screw axis of the $i$th joint expressed in the space frame when the manipulator is at the zero position. {\tt Slist} is a list of all the joint screw axes for the manipulator, $i = 1, \ldots, n$. \\ +$J_b, J_s$ & {\tt Jb}, {\tt Js} & The $6 \times n$ manipulator Jacobian for a robot with $n$ joints, expressed in the end-effector frame ($J_b$) or the space frame ($J_s$). \\ +${\epsilon}_{\omega}$ & {\tt eomg} & A small positive tolerance on the end-effector orientation error when calculating numerical inverse kinematics. \\ +${\epsilon}_{\nu}$ & {\tt ev} & A small positive tolerance on the end-effector linear position error when calculating numerical inverse kinematics. \\ +$\theta_0$ & {\tt thetalist0} & A list of joint variables that serve as an initial guess for the inverse kinematics solution.\\ +$\theta_i$ & {\tt thetalist} & $\theta_i$ is the joint variable for joint $i$, and {\tt thetalist} is $\theta = (\theta_1, \ldots, \theta_n)$.\\ +& {\tt thetamat} & An $N \times n$ matrix where each row represents $\theta$ one timestep after the row preceding it in the matrix. \\ +$\dot{\theta}_i$ & {\tt dthetalist} & $\dot{\theta}_i$ is the rate of change of joint variable $i$, and {\tt dthetalist} is $\dot{\theta} = (\dot{\theta}_1, \ldots, \dot{\theta}_n)$. \\ +& {\tt dthetamat} & An $N \times n$ matrix where each row represents $\dot{\theta}$ one timestep after the row preceding it in the matrix. \\ +$\ddot{\theta}_i$ & {\tt ddthetalist} & $\ddot{\theta}_i$ is the acceleration of joint $i$, and {\tt ddthetalist} is $\ddot{\theta} = (\ddot{\theta}_1, \ldots, \ddot{\theta}_n)$. \\ +& {\tt dthetamat} & An $N \times n$ matrix where each row represents $\ddot{\theta}$ one timestep after the row preceding it in the matrix. \\ +$\mathfrak{g}$ & {\tt g} & $3$-vector for gravitational acceleration. \\ +$\tilde{\mathfrak{g}}$ & {\tt gtilde} & A possibly incorrect model for $\mathfrak{g}$ used by a controller. \\ +$M_{i-1,i} $ & {\tt Mlist} & $M_{i-1,i} \in SE(3)$ is the configuration of manipulator link $i$ relative to link $i-1$ when the manipulator is at its zero position. The link frames are defined at the link centers of mass. {\tt Mlist} is a list of all $M_{i-1,i}$ for $i = 1, \ldots, n+1$. The frame $\{n+1\}$ is the end-effector frame, and it is fixed relative to the frame $\{n\}$ of the last link. It simply offers the opportunity to define an end-effector frame other than at the center of mass of the last link. \\ +& {\tt Mtildelist} & A possibly incorrect model for {\tt Mlist} used by a controller. \\ +$\mathcal{F}_{\text{tip}}$ & {\tt Ftip} & $6$-vector wrench applied by the manipulator end-effector, expressed in the end-effector frame $\{n+1\}$. \\ +& {\tt Ftipmat} & An $N \times 6$ matrix where each row represents $\mathcal{F}_{\text{tip}}$ one timestep after the row preceding it in the matrix. \\ +$\mathcal{G}_i$ & {\tt Glist} & $\mathcal{G}_i$ is the $6\times 6$ spatial inertia matrix for link $i$ of the manipulator, and {\tt Glist} is a list of all $\mathcal{G}_i$ for $i = 1, \ldots, n$. \\ +& {\tt Gtildelist} & A possibly incorrect model for {\tt Glist} used by a controller. \\ +$\tau_i$ & {\tt taulist} & $\tau_i$ is the generalized force applied at joint $i$, and {\tt taulist} is the list of all joint forces/torques $\tau = (\tau_1, \ldots, \tau_n)$. \\ +& {\tt taumat} & An $N \times n$ matrix where each row represents $\tau$ one timesetep after the row preceding it in the matrix. \\ +$[\operatorname{ad}_{\mathcal{V}}]$ & {\tt adV} & $6 \times 6$ matrix adjoint representation of $\mathcal{V} \in se(3)$, used to calculate the Lie bracket of two twists, $[\operatorname{ad}_{\mathcal{V}_1}] \mathcal{V}_2$. \\ +$T_f$ & {\tt Tf} & The total time of a motion in seconds from rest to rest when calculating trajectories. \\ +$\Delta t$ & {\tt dt} & A timestep (e.g., between consecutive rows in a matrix representing a trajectory or force history). \\ +$t$ & {\tt t} & The current time. \\ +$\theta_{\text{start}}$ & {\tt thetastart} & An $n$-vector of initial joint variables with which to start a trajectory.\\ +$\theta_{\text{end}}$ & {\tt thetaend} & An $n$-vector of final joint variables with which to end a trajectory.\\ +$X_{\text{start}}$ & {\tt Xstart} & An initial end-effector configuration $X_{\text{start}} \in SE(3)$ with which to start a trajectory.\\ +$X_{\text{end}}$ & {\tt Xend} & A final end-effector configuration $X_{\text{end}} \in SE(3)$ with which to end a trajectory.\\ +$e_{\text{int}}$ & {\tt eint} & An $n$-vector of the time-integral of joint errors.\\ +$\theta_d$ & {\tt thetalistd} & An $n$-vector of reference joint variables $\theta_d$.\\ +& {\tt thetamatd} & An $N \times n$ matrix where each row represents $\theta_d$ one timestep after the row preceding it in the matrix. \\ +$\dot{\theta}_d$ & {\tt dthetalistd} & An $n$-vector of reference joint velocities $\dot{\theta}_d$.\\ +& {\tt dthetamatd} & An $N \times n$ matrix where each row represents $\dot{\theta}_d$ one timestep after the row preceding it in the matrix. \\ +$\ddot{\theta}_d$ & {\tt ddthetalistd} & An $n$-vector of reference joint accelerations $\ddot{\theta}_d$.\\ +& {\tt dthetamatd} & An $N \times n$ matrix where each row represents $\ddot{\theta}_d$ one timestep after the row preceding it in the matrix. \\ +$K_p$ & {\tt Kp} & A scalar feedback proportional gain.\\ +$K_i$ & {\tt Ki} & A scalar feedback integral gain.\\ +$K_d$ & {\tt Kd} & A scalar feedback derivative gain.\\ +& {\tt intRes} & The number of integration steps during each timestep $\Delta t$. The value must be a positive integer. Larger values result in slower simulations but less accumulation of integration error. \\ +\bottomrule +\end{tabularx} + +\section*{Chapter 3: Rigid-Body Motions} %Chapter 3 + +\begin{function} %RotInv +{invR = RotInv(R)} +{\item \verb~R~: Rotation matrix.} +{\item \verb~invR~: The inverse of {\tt R}.} +{For efficiency, the inverse is calculated as the transpose rather than a matrix inverse.} +\end{function} + +\begin{function} %VecToso3 +{so3mat = VecToso3(omg)} +{\item \verb~omg~: A $3$-vector.} +{\item \verb~so3mat~: The corresponding $3 \times 3$ skew-symmetric matrix in $so(3)$.} +{} +\end{function} + +\begin{function} %so3ToVec +{omg = so3ToVec(so3mat)} +{\item \verb~so3mat~: A $3 \times 3$ skew-symmetric matrix (an element of $so(3)$).} +{\item \verb~omg~: The corresponding $3$-vector.} +{} +\end{function} + +\begin{function} %AxisAng3 +{[omghat,theta] = AxisAng3(expc3)} +{\item \verb~expc3~: A $3$-vector of exponential coordinates for rotation $\hat{\omega}\theta$.} +{ +\item \verb~omghat~: The corresponding unit rotation axis $\hat{\omega}$. +\item \verb~theta~: The corresponding rotation angle $\theta$. +} +{} +\end{function} + +\begin{function} %MatrixExp3 +{R = MatrixExp3(so3mat)} +{\item \verb~so3mat~: An $so(3)$ representation of exponential coordinates for rotation, $[\hat{\omega} \theta]$.} +{\item \verb~R~: The $R^\prime \in SO(3)$ that is achieved by rotating about $\hat{\omega}$ by $\theta$ from an initial orientation $R=I$.} +{} +\end{function} + +\begin{function} %MatrixLog3 +{so3mat = MatrixLog3(R)} +{\item \verb~R~: Rotation matrix.} +{\item \verb~so3mat~: The corresponding $so(3)$ representation of exponential coordinates.} +{} +\end{function} + +\begin{function} %DistanceToSO3 +{d = DistanceToSO3(mat)} +{\item \verb~mat~: A $3 \times 3$ matrix $M$.} +{\item \verb~d~: A measure of the distance from $M$ to $SO(3)$, the space of rotation matrices. If ${\rm det}(M)>0$ (the determinant of $M$ should be $1$ if $M \in SO(3)$), this distance is calculated as $\|M^\trans M -I\|_F$, since $M^\trans M$ should be the identity matrix if $M\in SO(3)$. The Frobenius norm $\| \cdot \|_F$ of a matrix is the square root of the sum of the squares of the absolute values of the elements of the matrix. If the determinant is not positive, a large value is returned. } +{} +\end{function} + +\begin{function} %TestIfSO3 +{judge = TestIfSO3(mat)} +{\item \verb~mat~: A $3 \times 3$ matrix $M$.} +{\item \verb~judge~: $1$ (True) if $M$ is a rotation matrix (an element of $SO(3)$) and $0$ (False) otherwise. This function calls \verb~DistanceToSO3(mat)~ and tests if the returned distance is smaller than a small value (which you should feel free to change to suit your purposes).} +{} +\end{function} + +\begin{function} %ProjectToSO3 +{R = ProjectToSO3(mat)} +{\item \verb~mat~: A $3 \times 3$ matrix $M$.} +{\item \verb~R~: The closest rotation matrix (element of $SO(3)$) to $M$. This function is only appropriate for matrices $M$ that are ``close'' to $SO(3)$. For example, $M$ could be the result of a long series of multiplications of rotation matrices, which has caused the result to drift slightly away from satisfying the conditions of $SO(3)$ (${\rm det}(M) = 1, M^\trans M = I$) due to roundoff errors.} +{} +\end{function} + +\begin{function} %RpToTrans +{T = RpToTrans(R,p)} +{ +\item \verb~R~: Rotation matrix. +\item \verb~p~: A position $p \in \real^3$. +} +{\item \verb~T~: The corresponding homogeneous transformation matrix $T \in SE(3)$.} +{} +\end{function} + +\begin{function} %TransToRp +{[R,p] = TransToRp(T)} +{\item \verb~T~: Transformation matrix.} +{ +\item \verb~R~: The corresponding rotation matrix. +\item \verb~p~: The corresponding position. +} +{} +\end{function} + +\begin{function} %TransInv +{invT = TransInv(T)} +{\item \verb~T~: Transformation matrix.} +{\item \verb~invT~: Inverse of \verb~T~.} +{Uses the structure of transformation matrices to avoid taking a matrix inverse, for efficiency.} +\end{function} + +\begin{function} %VecTose3 +{se3mat = VecTose3(V)} +{\item \verb~V~: A $6$-vector (representing a twist, for example).} +{\item \verb~se3mat~: The corresponding $4 \times 4$ $se(3)$ matrix.} +{} +\end{function} + +\begin{function} %se3ToVec +{V = se3ToVec(se3mat)} +{\item \verb~se3mat~: A $4 \times 4$ $se(3)$ matrix.} +{\item \verb~V~: The corresponding $6$-vector.} +{} +\end{function} + +\begin{function} %Adjoint +{AdT = Adjoint(T)} +{\item \verb~T~: Transformation matrix.} +{\item \verb~AdT~: The corresponding $6 \times 6$ adjoint representation $[\operatorname{Ad}_T]$.} +{} +\end{function} + +\begin{function} %ScrewToAxis +{S = ScrewToAxis(q,s,h)} +{ +\item \verb~q~: A point $q \in \real^3$ lying on the screw axis. +\item \verb~s~: A unit vector $\hat{s} \in \real^3$ in the direction of the screw axis. +\item \verb~h~: The pitch $h \in \real$ (linear velocity divided by angular velocity) of the screw axis. +} +{\item \verb~S~: The corresponding normalized screw axis ${\mathcal S} = (\omega,v)$.} +{} +\end{function} + +\begin{function} %AxisAng6 +{[S,theta] = AxisAng6(expc6)} +{\item \verb~expc6~: A $6$-vector of exponential coordinates for rigid-body motion, $\mathcal{S}\theta$.} +{ +\item \verb~S~: The corresponding normalized screw axis $\mathcal{S}$. +\item \verb~theta~: The distance traveled along/about $\mathcal{S}$. +} +{} +\end{function} + +\begin{function} %MatrixExp6 +{T = MatrixExp6(se3mat)} +{\item \verb~se3mat~: An $se(3)$ representation of exponential coordinates for rigid-body motion, $[\mathcal{S}\theta]$.} +{\item \verb~T~: The $T^\prime \in SE(3)$ that is achieved by traveling along/about the screw axis $\mathcal{S}$ a distance $\theta$ from an initial configuration $T=I$.} +{} +\end{function} + +\begin{function} %MatrixLog6 +{se3mat = MatrixLog6(T)} +{\item \verb~T~: Transformation matrix.} +{\item \verb~se3mat~: The corresponding $se(3)$ representation of exponential coordinates.} +{} +\end{function} + +\begin{function} %DistanceToSE3 +{d = DistanceToSE3(mat)} +{\item \verb~mat~: A $4 \times 4$ matrix $M$.} +{\item \verb~d~: A measure of the distance from $M$ to $SE(3)$, the + space of transformation matrices. Let $R$ be the top-left $3 \times 3$ submatrix of $M$, i.e., the portion of $M$ expected to represent a rotation matrix. If ${\rm det}(R)>0$ (the determinant of $R$ should be $1$ if $R \in SO(3)$), the distance is calculated as $\|M^\prime -I\|_F$, where the top-left $3 \times 3$ submatrix of $M^\prime$ is $R^\trans R$ (which should be the identity matrix if $R \in SO(3)$), the $1 \times 4$ bottom row of $M^\prime$ is the same as the bottom row of $M$, and the elements $M^\prime_{14}$, $M^\prime_{24}$, and $M^\prime_{34}$ are zero. The Frobenius norm $\| \cdot \|_F$ of a matrix is the the square root of the sum of the squares of the absolute values of the elements of the matrix. If the determinant of $R$ is not positive, a large value is returned. } +{} +\end{function} + +\begin{function} %TestIfSE3 +{judge = TestIfSE3(mat)} +{\item \verb~mat~: A $4 \times 4$ matrix $M$.} +{\item \verb~judge~: $1$ if $M$ is a transformation matrix (an element of $SE(3)$) and $0$ otherwise. This function calls \verb~DistanceToSE3(mat)~ and tests if the returned distance is smaller than a small value (which you should feel free to change to suit your purposes).} +{} +\end{function} + +\begin{function} %ProjectToSE3 +{T = ProjectToSE3(mat)} +{\item \verb~mat~: A $4 \times 4$ matrix $M$.} +{\item \verb~T~: The closest transformation matrix (element of $SE(3)$) to $M$. This function is only appropriate for matrices $M$ that are ``close'' to $SE(3)$. For example, $M$ could be the result of a long series of multiplications of transformation matrices, which has caused the result to drift slightly away from satisfying the conditions of $SE(3)$ (top-left $3\times 3$ submatrix is in $SO(3)$ and the bottom row is $[0 \; 0\; 0 \; 1]$) due to roundoff errors. The top-left $3 \times 3$ submatrix of $T$ is the $SO(3)$ matrix closest to the top-left $3 \times 3$ submatrix of $M$, the bottom row of $T$ is $[0 \; 0\; 0\; 1]$, and the elements $T_{14}$, $T_{24}$, and $T_{34}$ are equivalent to the elements $M_{14}$, $M_{24}$, and $M_{34}$, respectively. } +{} +\end{function} + +\section*{Chapter 4: Forward Kinematics} %Chapter 4 + +\begin{function} %FKinBody +{T = FKinBody(M,Blist,thetalist)} +{ +\item \verb~M~: The home configuration of the end-effector. +\item \verb~Blist~: The joint screw axes in the end-effector frame when the manipulator is at the home position. +\item \verb~thetalist~: A list of joint coordinate values. +} +{\item \verb~T~: The $T \in SE(3)$ representing the end-effector frame when the joints are at the specified coordinates.} +{} +\end{function} + +\begin{function} %FKinSpace +{T = FKinSpace(M,Slist,thetalist)} +{ +\item \verb~M~: The home configuration of the end-effector. +\item \verb~Slist~: The joint screw axes in the space frame when the manipulator is at the home position. +\item \verb~thetalist~: A list of joint coordinate values. +} +{\item \verb~T~: The $T \in SE(3)$ representing the end-effector frame when the joints are at the specified coordinates.} +{} +\end{function} + +\section*{Chapter 5: Velocity Kinematics and Statics} %Chapter 5 + +\begin{function} %JacobianBody +{Jb = JacobianBody(Blist,thetalist)} +{ +\item \verb~Blist~: The joint screw axes in the end-effector frame when the manipulator is at the home position. +\item \verb~thetalist~: A list of joint coordinate values. +} +{\item \verb~Jb~: The corresponding body Jacobian $J_b(\theta) \in \real^{6 \times n}$.} +{} +\end{function} + +\begin{function} %JacobianSpace +{Js = JacobianSpace(Slist,thetalist)} +{ +\item \verb~Slist~: The joint screw axes in the space frame when the manipulator is at the home position. +\item \verb~thetalist~: A list of joint coordinate values. +} +{\item \verb~Js~: The corresponding space Jacobian $J_s(\theta) \in \real^{6 \times n}$.} +{} +\end{function} + +\section*{Chapter 6: Inverse Kinematics} %Chapter 6 + +\begin{function} %IKinBody +{[thetalist,success] = IKinBody(Blist,M,T,thetalist0,eomg,ev)} +{ +\item \verb~Blist~: The joint screw axes in the end-effector frame when the manipulator is at the home position. +\item \verb~M~: The home configuration of the end-effector. +\item \verb~T~: The desired end-effector configuration $T_{sd}$. +\item \verb~thetalist0~: An initial guess $\theta_0 \in \real^n$ that is ``close'' to satisfying $T(\theta_0) = T_{sd}$. +\item \verb~eomg~: A small positive tolerance on the end-effector orientation error. The returned joint variables must give an end-effector orientation error less than ${\epsilon}_{\omega}$. +\item \verb~ev~: A small positive tolerance on the end-effector linear position error. The returned joint variables must give an end-effector position error less than ${\epsilon}_{\nu}$. +} +{ +\item \verb~thetalist~: Joint variables that achieve \verb~T~ within the specified tolerances. +\item \verb~success~: A logical value where \verb~TRUE~ means that the function found a solution and \verb~FALSE~ means that it ran through the set number of maximum iterations without finding a solution within the tolerances ${\epsilon}_{\omega}$ and ${\epsilon}_{\nu}$. +} +{The algorithm uses an iterative Newton-Raphson root-finding method starting from the initial guess {\tt thetalist0}. The algorithm terminates when the stopping criteria are met or after a maximum number of iterations, whichever comes first. The maximum number of iterations has been hardcoded in as a variable in the function, which can be changed if desired. If the stopping criteria are not met, the function returns the last calculation of \verb~thetalist~ as well as a \verb~FALSE~ value for the success variable.} +\end{function} + +\begin{function} %IKinSpace +{[thetalist,success] = IKinSpace(Slist,M,T,thetalist0,eomg,ev)} +{} +{} +{Equivalent to \verb~IKinBody~, except the joint screw axes are specified in the space frame.} +\end{function} + + +\section*{Chapter 8: Dynamics of Open Chains} %Chapter 8 + +This chapter is concerned with calculating and simulating the dynamics of a serial-chain manipulator with dynamics of the form +\[ +\tau = M(\theta)\ddot{\theta} + c(\theta,\dot{\theta}) + g(\theta) + J^\trans(\theta) \mathcal{F}_{\text{tip}}. +\] + +\begin{function} %ad +{adV = ad(V)} +{\item \verb~V~: A $6$-vector (e.g., a twist).} +{\item \verb~adV~: The corresponding $6 \times 6$ matrix $[\operatorname{ad}_{\mathcal{V}}]$.} +{Used to calculate the Lie bracket $[\operatorname{ad}_{\mathcal{V}_1}]\mathcal{V}_2$.} +\end{function} + +\begin{function} %InverseDynamics +{taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,\\ +\hspace*{1in} g,Ftip,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~dthetalist~: $n$-vector of joint velocities $\dot{\theta}$. +\item \verb~ddthetalist~: $n$-vector of joint accelerations $\ddot{\theta}$. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Ftip~: Wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~taulist~: The $n$-vector $\tau$ of required joint forces/torques.} +{This function uses forward-backward Newton-Euler iterations.} +\end{function} + +\begin{function} %MassMatrix +{M = MassMatrix(thetalist,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~M~: The numerical inertia matrix $M(\theta)$ of an $n$-joint serial chain at the given configuration $\theta$.} +{This function calls {\tt InverseDynamics} $n$ times, each time passing a $\ddot{\theta}$ vector with a single element equal to one and all other inputs set to zero. Each call of {\tt InverseDynamics} generates a single column of the robot's mass matrix, and these columns are assembled to create the full mass matrix.} +\end{function} + +\begin{function} %VelQuadraticForces +{c = VelQuadraticForces(thetalist,dthetalist,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~dthetalist~: $n$-vector of joint velocities $\dot{\theta}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~c~: The vector $c(\theta,\dot{\theta})$ of Coriolis and centripetal terms for a given $\theta$ and $\dot{\theta}$.} +{This function calls {\tt InverseDynamics} with $\mathfrak{g}=0$, $\mathcal{F}_{\textrm{tip}} = 0$, and $\ddot{\theta} = 0$.} +\end{function} + +\begin{function} %GravityForces +{grav = GravityForces(thetalist,g,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~grav~: The joint forces/torques required to balance gravity at $\theta$.} +{This function calls \verb~InverseDynamics~ with $\dot{\theta} = \ddot{\theta} = 0$ and $\mathcal{F}_{\textrm{tip}} = 0$.} +\end{function} + +\begin{function} %EndEffectorForces +{JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~Ftip~: Wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~JTFtip~: The joint forces and torques $J^\trans(\theta) \mathcal{F}_{\text{tip}}$ required to create the end-effector force $\mathcal{F}_{\text{tip}}$.} +{This function calls \verb~InverseDynamics~ with $\mathfrak{g}=0$ and $\dot{\theta} = \ddot{\theta} = 0$.} +\end{function} + +\begin{function} %ForwardDynamics +{ddthetalist = ForwardDynamics(thetalist,dthetalist,taulist,\\ +\hspace*{1in} g,Ftip,Mlist,Glist,Slist)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~dthetalist~: $n$-vector of joint velocities $\dot{\theta}$. +\item \verb~taulist~: The $n$-vector $\tau$ of required joint forces/torques. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Ftip~: Wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~ddthetalist~: The resulting joint accelerations $\ddot{\theta}$.} +{This function computes $\ddot{\theta}$ by solving +\[ +M(\theta) \ddot{\theta} = \tau - c(\theta,\dot{\theta}) - g(\theta) - J^\trans(\theta) \mathcal{F}_{\textrm{tip}}. +\] +} +\end{function} + +\begin{function} %EulerStep +{[thetalistNext,dthetalistNext] = EulerStep(thetalist,dthetalist,ddthetalist,dt)} +{ +\item \verb~thetalist~: $n$-vector of joint variables $\theta$. +\item \verb~dthetalist~: $n$-vector of joint velocities $\dot{\theta}$. +\item \verb~ddthetalist~: $n$-vector of joint accelerations $\ddot{\theta}$. +\item \verb~dt~: The timestep $\Delta t$. +} +{ +\item \verb~thetalistNext~: Vector of joint variables $\theta$ after $\Delta t$ from first-order Euler integration. +\item \verb~dthetalistNext~: Vector of joint velocities $\dot{\theta}$ after $\Delta t$ from first-order Euler integration. +} +{ +} +\end{function} + +\begin{function} %InverseDynamicsTrajectory +{taumat = InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat, \\ +\hspace*{1in} g,Ftipmat,Mlist,Glist,Slist)} +{ +\item \verb~thetamat~: An $N \times n$ matrix of robot joint variables. Each row is an $n$-vector of joint variables, and the $N$ rows correspond to $N$ time instants. (The time instants can be thought of as starting at time $0$ and ending at time $T_f$, in increments $\Delta t = T_f/(N-1)$.) +\item \verb~dthetamat~: An $N \times n$ matrix of robot joint velocities. +\item \verb~ddthetamat~: An $N \times n$ matrix of robot joint accelerations. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Ftipmat~: An $N \times 6$ matrix, where each row is a vector of the form $\mathcal{F}_{\textrm{tip}}(k \Delta t)$. (If there are no tip forces the user should input a zero and a zero matrix will be used). +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +} +{\item \verb~taumat~: The $N \times n$ matrix of joint forces/torques for the specified trajectory, where each of the $N$ rows is the vector of joint forces/torques at each time step.} +{This function uses \verb~InverseDynamics~ to calculate the joint forces/torques required to move the serial chain along the given trajectory.} +\end{function} + +\begin{function} %ForwardDynamicsTrajectory +{[thetamat,dthetamat] = ForwardDynamicsTrajectory(thetalist,dthetalist,taumat, \\ +\hspace*{1in} g,Ftipmat,Mlist,Glist,Slist,dt,intRes)} +{ +\item \verb~thetalist~: $n$-vector of initial joint variables. +\item \verb~dthetalist~: $n$-vector of initial joint velocities. +\item \verb~taumat~: An $N \times n$ matrix of joint forces/torques, where each row is the joint force/torque at any instant. The time corresponding to row $k$ is $k\Delta t$, $k \in \{0, \ldots, N-1\}$, where $\Delta t$ is defined below. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Ftipmat~: An $N \times 6$ matrix, where each row is a vector of the form $\mathcal{F}_{\textrm{tip}}(k \Delta t)$. (If there are no tip forces the user should input a zero and a zero matrix will be used). +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +\item \verb~dt~: The timestep $\Delta t$ between consecutive joint forces/torques. +\item \verb~intRes~: This input must be an integer greater than or equal to 1. {\tt intRes} is the number of Euler integration steps during each timestep $\Delta t$. Larger values result in slower simulations but less accumulation of integration error. } +{ +\item \verb~thetamat~: The $N \times n$ matrix of robot joint variables resulting from the specified joint forces/torques. +\item \verb~dthetamat~: The $N \times n$ matrix of robot joint velocities resulting from the specified joint forces/torques. +} +{This function simulates the motion of a serial chain given an open-loop history of joint forces/torques. It calls a numerical integration procedure that uses \verb~ForwardDynamics~.} +\end{function} + + +\section*{Chapter 9: Trajectory Generation} %Chapter 9 + +\begin{function} %CubicTimeScaling +{s = CubicTimeScaling(Tf,t)} +{ +\item \verb~Tf~: Total time of the motion $T_f$ in seconds from rest to rest. +\item \verb~t~: The current time $t$ satisfying $0 \leq t \leq T_f$. +} +{\item \verb~s~: The path parameter $s(t)$ corresponding to a third-order polynomial motion that begins (at $s(0)=0$) and ends (at $s(T_f)=1$) at zero velocity.} +{} +\end{function} + +\begin{function} %QuinticTimeScaling +{s = QuinticTimeScaling(Tf,t)} +{ +\item \verb~Tf~: Total time of the motion $T_f$ in seconds from rest to rest. +\item \verb~t~: The current time $t$ satisfying $0 \leq t \leq T_f$. +} +{\item \verb~s~: The path parameter $s(t)$ corresponding to a fifth-order polynomial motion that begins (at $s(0)=0$) and ends (at $s(T_f)=1$) at zero velocity and zero acceleration.} +{} +\end{function} + +\begin{function} %JointTrajectory +{traj = JointTrajectory(thetastart,thetaend,Tf,N,method)} +{ +\item \verb~thetastart~: The initial joint variables $\theta_{\text{start}} \in \real^n$. +\item \verb~thetaend~: The final joint variables $\theta_{\text{end}}$. +\item \verb~Tf~: Total time of the motion $T_f$ in seconds from rest to rest. +\item \verb~N~: The number of points $N \geq 2$ in the discrete representation of the trajectory. +\item \verb~method~: The time-scaling method, where 3 indicates cubic (third-order polynomial) time scaling and 5 indicates quintic (fifth-order polynomial) time scaling. +} +{\item \verb~traj~: A trajectory as an $N \times n$ matrix, where each row is an $n$-vector of joint variables at an instant in time. The first row is $\theta_{\text{start}}$ and the $N$th row is $\theta_{\text{end}}$. The elapsed time between each row is $T_f/(N-1)$. } +{The returned trajectory is a straight-line motion in joint space.} +\end{function} + +\begin{function} %ScrewTrajectory +{traj = ScrewTrajectory(Xstart,Xend,Tf,N,method)} +{ +\item \verb~Xstart~: The initial end-effector configuration $X_{\text{start}} \in SE(3)$. +\item \verb~Xend~: The final end-effector configuration $X_{\text{end}}$. +\item \verb~Tf~: Total time of the motion $T_f$ in seconds from rest to rest. +\item \verb~N~: The number of points $N \geq 2$ in the discrete representation of the trajectory. +\item \verb~method~: The time-scaling method, where 3 indicates cubic (third-order polynomial) time scaling and 5 indicates quintic (fifth-order polynomial) time scaling. +} +{\item \verb~traj~: The discretized trajectory as a list of $N$ matrices in $SE(3)$ separated in time by $T_f/(N-1)$. The first in the list is $X_{\text{start}}$ and the $N$th is $X_{\text{end}}$.} +{This function calculates a trajectory corresponding to a screw motion about a constant screw axis.} +\end{function} + +\begin{function} %CartesianTrajectory +{traj = CartesianTrajectory(Xstart,Xend,Tf,N,method)} +{ +\item \verb~Xstart~: The initial end-effector configuration $X_{\text{start}} \in SE(3)$. +\item \verb~Xend~: The final end-effector configuration $X_{\text{end}}$. +\item \verb~Tf~: Total time of the motion $T_f$ in seconds from rest to rest. +\item \verb~N~: The number of points $N \geq 2$ in the discrete representation of the trajectory. +\item \verb~method~: The time-scaling method, where 3 indicates cubic (third-order polynomial) time scaling and 5 indicates quintic (fifth-order polynomial) time scaling. +} +{\item \verb~traj~: The discretized trajectory as a list of $N$ matrices in $SE(3)$ separated in time by $T_f/(N-1)$. The first in the list is $X_{\text{start}}$ and the $N$th is $X_{\text{end}}$.} +{Similar to {\tt ScrewTrajectory}, except the origin of the end-effector frame follows a straight line, decoupled from the rotational motion.} +\end{function} + + +\section*{Chapter 11: Robot Control} %Chapter 11 + +The two functions in this chapter focus on the use of the computed torque controller +\[ +\tau = \widehat{M}(\theta) \left(\ddot{\theta}_d + K_p \theta_e + K_i \int \theta_e(t) dt + K_d \dot{\theta}_e\right) + \widehat{h}(\theta,\dot{\theta}) +\] +to control the motion of a serial chain in free space. The term $\widehat{h}(\theta,\dot{\theta})$ comprises the model of centripetal, Coriolis, and gravitational forces, and the term $\widehat{M}(\theta)$ is the model of the robot's mass matrix. + +\begin{function} %ComputedTorque +{taulist = ComputedTorque(thetalist,dthetalist,eint,g,\\ +\hspace*{1in} Mlist,Glist,Slist,thetalistd,dthetalistd,ddthetalistd,Kp,Ki,Kd)} +{ +\item \verb~thetalist~: $n$-vector of initial joint variables. +\item \verb~dthetalist~: $n$-vector of initial joint velocities. +\item \verb~eint~: An $n$-vector of the time-integral of joint errors. +\item \verb~g~: Gravity vector $\mathfrak{g}$. +\item \verb~Mlist~: List of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +\item \verb~thetalistd~: $n$-vector of reference joint variables $\theta_d$. +\item \verb~dthetalistd~: $n$-vector of reference joint velocities $\dot{\theta}_d$. +\item \verb~ddthetalistd~: $n$-vector of reference joint accelerations $\ddot{\theta}_d$. +\item \verb~Kp~: The feedback proportional gain (identical for each joint). +\item \verb~Ki~: The feedback integral gain (identical for each joint). +\item \verb~Kd~: The feedback derivative gain (identical for each joint). +} +{\item \verb~taulist~: The vector of joint forces/torques computed by the computed torque controller at the current instant.} +{} +\end{function} + +\begin{function} %SimulateControl +{[taumat,thetamat] = SimulateControl(thetalist,dthetalist,g,Ftipmat,Mlist,Glist, \\ +\hspace*{1in} Slist,thetamatd,dthetamatd,ddthetamatd,gtilde,Mtildelist, \\ +\hspace*{1in} Gtildelist,Kp,Ki,Kd,dt,intRes)} +{ +\item \verb~thetalist~: $n$-vector of initial joint variables. +\item \verb~dthetalist~: $n$-vector of initial joint velocities. +\item \verb~g~: Actual gravity vector $\mathfrak{g}$. +\item \verb~Ftipmat~: An $N \times 6$ matrix, where each row is a vector of the form $\mathcal{F}_{\textrm{tip}}(k \Delta t)$. (If there are no tip forces the user should input a zero and a zero matrix will be used). +\item \verb~Mlist~: Actual list of link frames $\{i\}$ relative to $\{i-1\}$ at the home position. +\item \verb~Glist~: Actual spatial inertia matrices $\mathcal{G}_i$ of the links. +\item \verb~Slist~: Screw axes $\mathcal{S}_i$ of the joints in a space frame. +\item \verb~thetamatd~: An $N \times n$ matrix of desired joint variables $\theta_d$ from the reference trajectory. The first row is the initial desired joint configuration, and the $N$th row is the final desired joint configuration. The time between each row is {\tt dt}, below. +\item \verb~dthetamatd~: An $N \times n$ matrix of desired joint velocities $\dot{\theta}_d$. +\item \verb~ddthetamatd~: An $N \times n$ matrix of desired joint accelerations $\ddot{\theta}_d$. +\item \verb~gtilde~: The (possibly incorrect) model of the gravity vector. +\item \verb~Mtildelist~: The (possibly incorrect) model of the link frame locations. +\item \verb~Gtildelist~: The (possibly incorrect) model of the link spatial inertias. +\item \verb~Kp~: The feedback proportional gain (identical for each joint). +\item \verb~Ki~: The feedback integral gain (identical for each joint). +\item \verb~Kd~: The feedback derivative gain (identical for each joint). +\item \verb~dt~: The timestep $\Delta t$ between points on the reference trajectory. +\item \verb~intRes~: This input must be an integer greater than or equal to 1. {\tt intRes} is the number of Euler integration steps during each timestep $\Delta t$. Larger values result in slower simulations but less accumulation of integration error. +} +{ +\item \verb~taumat~: An $N \times n$ matrix of the controller's commanded joint forces/torques, where each row of $n$ forces/torques corresponds to a single time instant. +\item \verb~thetamat~: An $N \times n$ matrix of actual joint variables, to be compared to \verb~thetamatd~. +\item Plot: Plot of actual and desired joint variables. +} +{This function uses {\tt ComputedTorque}, {\tt ForwardDynamics}, and numerical integration to simulate the performance of a computed torque control law operating on a serial chain. Disturbances come in the form of initial position and velocity errors; incorrect models of gravity, the locations of the link center of mass frames, and the link spatial inertias; and errors introduced by the numerical integration.} +\end{function} + + +\end{document} diff --git a/packages/MATLAB/README.md b/packages/MATLAB/README.md new file mode 100644 index 0000000..1674098 --- /dev/null +++ b/packages/MATLAB/README.md @@ -0,0 +1,56 @@ +# "mr" MATLAB Code Library Instructions # + +## Installing the Library ## + +Copy the "mr" folder to a known location on your computer. We will call the +path to this folder `$FOLDER_PATH/mr`. + +## Importing the Library ## + +To import the library, use `addpath` as + +``` +addpath('$FOLDER_PATH/mr') +``` + +This process is required for any program using this package. + +## Using the Package ## + +After importing the library, you should be able to use any function in the +library. Taking the function `RotInv` for example, you can check the +description and usage example of this function by running + +``` +help RotInv +``` + +As mentioned in the function usage example, you can try using this function +by running + +``` +R = [0, 0, 1; 1, 0, 0; 0, 1, 0]; +invR = RotInv(R); +``` + +You should get the the variable `invR` whose value is the same as the output +shown in the function usage example. + +To check the function list and which chapter in the book those functions +belong to, use `help` as + +``` +help mr +``` + +## Library Information ## + +Author: Huan Weng, Bill Hunt, Mikhail Todes, Jarvis Schultz + +Contact: huanweng@u.northwestern.edu + +Package Version: 1.1.0 + +Matlab Version: R2017b + +Tested in Matlab R2017b diff --git a/packages/MATLAB/mr/Adjoint.m b/packages/MATLAB/mr/Adjoint.m new file mode 100644 index 0000000..ecb5c42 --- /dev/null +++ b/packages/MATLAB/mr/Adjoint.m @@ -0,0 +1,22 @@ +function AdT = Adjoint(T) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes T a transformation matrix SE3. +% Returns the corresponding 6x6 adjoint representation [AdT]. +% Example Input: +% +% clear; clc; +% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; +% AdT = Adjoint(T) +% +% Output: +% AdT = +% 1 0 0 0 0 0 +% 0 0 -1 0 0 0 +% 0 1 0 0 0 0 +% 0 0 3 1 0 0 +% 3 0 0 0 0 -1 +% 0 0 0 0 1 0 + +[R, p] = TransToRp(T); +AdT = [R, zeros(3); VecToso3(p) * R, R]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/AxisAng3.m b/packages/MATLAB/mr/AxisAng3.m new file mode 100644 index 0000000..9aad061 --- /dev/null +++ b/packages/MATLAB/mr/AxisAng3.m @@ -0,0 +1,22 @@ +function [omghat, theta] = AxisAng3(expc3) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes A 3-vector of exponential coordinates for rotation. +% Returns the unit rotation axis omghat and the corresponding rotation +% angle theta. +% Example Input: +% +% clear; clc; +% expc3 = [1; 2; 3]; +% [omghat, theta] = AxisAng3(expc3) +% +% Output: +% omghat = +% 0.2673 +% 0.5345 +% 0.8018 +% theta = +% 3.7417 + +theta = norm(expc3); +omghat = expc3 / theta; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/AxisAng6.m b/packages/MATLAB/mr/AxisAng6.m new file mode 100644 index 0000000..c103433 --- /dev/null +++ b/packages/MATLAB/mr/AxisAng6.m @@ -0,0 +1,29 @@ +function [S, theta] = AxisAng6(expc6) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 6-vector of exponential coordinates for rigid-body motion +% S*theta. +% Returns S: the corresponding normalized screw axis, +% theta: the distance traveled along/about S. +% Example Input: +% +% clear; clc; +% expc6 = [1; 0; 0; 1; 2; 3]; +% [S, theta] = AxisAng6(expc6) +% +% Output: +% S = +% 1 +% 0 +% 0 +% 1 +% 2 +% 3 +% theta = +% 1 + +theta = norm(expc6(1: 3)); +if NearZero(theta) + theta = norm(expc6(4: 6)); +end +S = expc6 / theta; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/CartesianTrajectory.m b/packages/MATLAB/mr/CartesianTrajectory.m new file mode 100644 index 0000000..3c2e7e5 --- /dev/null +++ b/packages/MATLAB/mr/CartesianTrajectory.m @@ -0,0 +1,63 @@ +function traj = CartesianTrajectory(Xstart, Xend, Tf, N, method) +% *** CHAPTER 9: TRAJECTORY GENERATION *** +% Takes Xstart: The initial end-effector configuration, +% Xend: The final end-effector configuration, +% Tf: Total time of the motion in seconds from rest to rest, +% N: The number of points N > 1 (Start and stop) in the discrete +% representation of the trajectory, +% method: The time-scaling method, where 3 indicates cubic +% (third-order polynomial) time scaling and 5 indicates +% quintic (fifth-order polynomial) time scaling. +% Returns traj: The discretized trajectory as a list of N matrices in SE(3) +% separated in time by Tf/(N-1). The first in the list is +% Xstart and the Nth is Xend . +% This function is similar to ScrewTrajectory, except the origin of the +% end-effector frame follows a straight line, decoupled from the rotational +% motion. +% Example Input: +% +% clear; clc; +% Xstart = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]]; +% Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]]; +% Tf = 5; +% N = 4; +% method = 5; +% traj = CartesianTrajectory(Xstart, Xend, Tf, N, method) +% +% Output: +% traj = +% 1.0000 0 0 1.0000 +% 0 1.0000 0 0 +% 0 0 1.0000 1.0000 +% 0 0 0 1.0000 +% +% 0.9366 -0.2140 0.2774 0.8111 +% 0.2774 0.9366 -0.2140 0 +% -0.2140 0.2774 0.9366 1.6506 +% 0 0 0 1.0000 +% +% 0.2774 -0.2140 0.9366 0.2889 +% 0.9366 0.2774 -0.2140 0 +% -0.2140 0.9366 0.2774 3.4494 +% 0 0 0 1.0000 +% +% -0.0000 0.0000 1.0000 0.1000 +% 1.0000 -0.0000 0.0000 0 +% 0.0000 1.0000 -0.0000 4.1000 +% 0 0 0 1.0000 + +timegap = Tf / (N - 1); +traj = cell(1, N); +[Rstart, pstart] = TransToRp(Xstart); +[Rend, pend] = TransToRp(Xend); +for i = 1: N + if method == 3 + s = CubicTimeScaling(Tf,timegap * (i - 1)); + else + s = QuinticTimeScaling(Tf,timegap * (i - 1)); + end + traj{i} ... + = [Rstart * MatrixExp3(MatrixLog3(Rstart' * Rend) * s), ... + pstart + s * (pend - pstart); 0, 0, 0, 1]; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ComputedTorque.m b/packages/MATLAB/mr/ComputedTorque.m new file mode 100644 index 0000000..4dbf8dc --- /dev/null +++ b/packages/MATLAB/mr/ComputedTorque.m @@ -0,0 +1,63 @@ +function taulist = ComputedTorque(thetalist, dthetalist, eint, g, ... + Mlist, Glist, Slist, thetalistd, ... + dthetalistd, ddthetalistd, Kp, Ki, Kd) +% *** CHAPTER 11: ROBOT CONTROL *** +% Takes thetalist: n-vector of joint variables, +% dthetalist: n-vector of joint rates, +% eint: n-vector of the time-integral of joint errors, +% g: Gravity vector g, +% Mlist: List of link frames {i} relative to {i-1} at the home +% position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns, +% thetalistd: n-vector of reference joint variables, +% dthetalistd: n-vector of reference joint velocities, +% ddthetalistd: n-vector of reference joint accelerations, +% Kp: The feedback proportional gain (identical for each joint), +% Ki: The feedback integral gain (identical for each joint), +% Kd: The feedback derivative gain (identical for each joint). +% Returns taulist: The vector of joint forces/torques computed by the +% feedback linearizing controller at the current instant. +% Example Input: +% +% clc; clear; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% eint = [0.2; 0.2; 0.2]; +% g = [0; 0; -9.8]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% thetalistd = [1; 1; 1]; +% dthetalistd = [2; 1.2; 2]; +% ddthetalistd = [0.1; 0.1; 0.1]; +% Kp = 1.3; +% Ki = 1.2; +% Kd = 1.1; +% taulist ... +% = ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, ... +% thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd) +% +% Output: +% taulist = +% 133.0053 +% -29.9422 +% -3.0328 + +e = thetalistd - thetalist; +taulist ... += MassMatrix(thetalist, Mlist, Glist, Slist) ... + * (Kp * e + Ki * (eint + e) + Kd * (dthetalistd - dthetalist)) ... + + InverseDynamics(thetalist, dthetalist, ddthetalistd, g, ... + zeros(6, 1), Mlist, Glist, Slist); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/CubicTimeScaling.m b/packages/MATLAB/mr/CubicTimeScaling.m new file mode 100644 index 0000000..dc66d46 --- /dev/null +++ b/packages/MATLAB/mr/CubicTimeScaling.m @@ -0,0 +1,19 @@ +function s = CubicTimeScaling(Tf, t) +% *** CHAPTER 9: TRAJECTORY GENERATION *** +% Takes Tf: Total time of the motion in seconds from rest to rest, +% t: The current time t satisfying 0 < t < Tf. +% Returns s: The path parameter s(t) corresponding to a third-order +% polynomial motion that begins and ends at zero velocity. +% Example Input: +% +% clear; clc; +% Tf = 2; +% t = 0.6; +% s = CubicTimeScaling(Tf,t) +% +% Output: +% s = +% 0.2160 + +s = 3 * (t / Tf) ^ 2 - 2 * (t / Tf) ^ 3; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/DistanceToSE3.m b/packages/MATLAB/mr/DistanceToSE3.m new file mode 100644 index 0000000..fd3296d --- /dev/null +++ b/packages/MATLAB/mr/DistanceToSE3.m @@ -0,0 +1,29 @@ +function d = DistanceToSE3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A 4x4 matrix. +% Returns the Frobenius norm to describe the distance of mat from the SE(3) +% manifold. +% Compute the determinant of matR, the top 3x3 submatrix of mat. If +% det(matR) <= 0, return a large number. If det(matR) > 0, replace the top +% 3x3 submatrix of mat with matR' * matR, and set the first three entries +% of the fourth column of mat to zero. Then return norm(mat - I). +% Example Inputs: +% +% clear; clc; +% mat = [1.0, 0.0, 0.0, 1.2; +% 0.0, 0.1, -0.95, 1.5; +% 0.0, 1.0, 0.1, -0.9; +% 0.0, 0.0, 0.1, 0.98]; +% d = DistanceToSE3(mat) +% +% Output: +% d = +% 0.1349 + +[R, p] = TransToRp(mat); +if det(R) > 0 + d = norm([R' * R, [0; 0; 0]; mat(4, :)] - eye(4), 'fro'); +else + d = 1e+9; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/DistanceToSO3.m b/packages/MATLAB/mr/DistanceToSO3.m new file mode 100644 index 0000000..1690236 --- /dev/null +++ b/packages/MATLAB/mr/DistanceToSO3.m @@ -0,0 +1,27 @@ +function d = DistanceToSO3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A 3x3 matrix. +% Returns the Frobenius norm to describe the distance of mat from the SO(3) +% manifold. +% Computes the distance from R to the SO(3) manifold using the following +% method: +% If det(mat) <= 0, return a large number. +% If det(mat) > 0, return norm(mat' * mat - I). +% Example Inputs: +% +% clear; clc; +% mat = [1.0, 0.0, 0.0; +% 0.0, 0.1, -0.95; +% 0.0, 1.0, 0.1]; +% d = DistanceToSO3(mat) +% +% Output: +% d = +% 0.0884 + +if det(mat) > 0 + d = norm(mat' * mat - eye(3), 'fro'); +else + d = 1e+9; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/EndEffectorForces.m b/packages/MATLAB/mr/EndEffectorForces.m new file mode 100644 index 0000000..32ea87a --- /dev/null +++ b/packages/MATLAB/mr/EndEffectorForces.m @@ -0,0 +1,42 @@ +function JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: A list of joint variables, +% Ftip: Spatial force applied by the end-effector expressed in frame +% {n+1}, +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with screw axes as the columns, +% Returns JTFtip: The joint forces and torques required only to create the +% end-effector force Ftip. +% This function calls InverseDynamics with g = 0, dthetalist = 0, and +% ddthetalist = 0. +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% Ftip = [1; 1; 1; 1; 1; 1]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist) +% +% Output: +% JTFtip = +% 1.4095 +% 1.8577 +% 1.3924 + +n = size(thetalist, 1); +JTFtip = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1), ... + [0; 0; 0], Ftip, Mlist, Glist, Slist); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/EulerStep.m b/packages/MATLAB/mr/EulerStep.m new file mode 100644 index 0000000..d1d8ab0 --- /dev/null +++ b/packages/MATLAB/mr/EulerStep.m @@ -0,0 +1,34 @@ +function [thetalistNext, dthetalistNext] ... + = EulerStep(thetalist, dthetalist, ddthetalist, dt) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: n-vector of joint variables, +% dthetalist: n-vector of joint rates, +% ddthetalist: n-vector of joint accelerations, +% dt: The timestep delta t. +% Returns thetalistNext: Vector of joint variables after dt from first +% order Euler integration, +% dthetalistNext: Vector of joint rates after dt from first order +% Euler integration. +% Example Inputs (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% ddthetalist = [2; 1.5; 1]; +% dt = 0.1; +% [thetalistNext, dthetalistNext] = EulerStep(thetalist, dthetalist, ... +% ddthetalist, dt) +% +% Output: +% thetalistNext = +% 0.1100 +% 0.1200 +% 0.1300 +% dthetalistNext = +% 0.3000 +% 0.3500 +% 0.4000 + +thetalistNext = thetalist + dt * dthetalist; +dthetalistNext = dthetalist + dt * ddthetalist; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/FKinBody.m b/packages/MATLAB/mr/FKinBody.m new file mode 100644 index 0000000..ce64e7f --- /dev/null +++ b/packages/MATLAB/mr/FKinBody.m @@ -0,0 +1,29 @@ +function T = FKinBody(M, Blist, thetalist) +% *** CHAPTER 4: FORWARD KINEMATICS *** +% Takes M: the home configuration (position and orientation) of the +% end-effector, +% Blist: The joint screw axes in the end-effector frame when the +% manipulator is at the home position, +% thetalist: A list of joint coordinates. +% Returns T in SE(3) representing the end-effector frame when the joints +% are at the specified coordinates (i.t.o Body Frame). +% Example Inputs: +% +% clear; clc; +% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; +% Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]]; +% thetalist = [pi / 2; 3; pi]; +% T = FKinBody(M, Blist, thetalist) +% +% Output: +% T = +% -0.0000 1.0000 0 -5.0000 +% 1.0000 0.0000 0 4.0000 +% 0 0 -1.0000 1.6858 +% 0 0 0 1.0000 + +T = M; +for i = 1: size(thetalist) + T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i))); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/FKinSpace.m b/packages/MATLAB/mr/FKinSpace.m new file mode 100644 index 0000000..8b3a780 --- /dev/null +++ b/packages/MATLAB/mr/FKinSpace.m @@ -0,0 +1,31 @@ +function T = FKinSpace(M, Slist, thetalist) +% *** CHAPTER 4: FORWARD KINEMATICS *** +% Takes M: the home configuration (position and orientation) of the +% end-effector, +% Slist: The joint screw axes in the space frame when the manipulator +% is at the home position, +% thetalist: A list of joint coordinates. +% Returns T in SE(3) representing the end-effector frame, when the joints +% are at the specified coordinates (i.t.o Space Frame). +% Example Inputs: +% +% clear; clc; +% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; +% Slist = [[0; 0; 1; 4; 0; 0], ... +% [0; 0; 0; 0; 1; 0], ... +% [0; 0; -1; -6; 0; -0.1]]; +% thetalist =[pi / 2; 3; pi]; +% T = FKinSpace(M, Slist, thetalist) +% +% Output: +% T = +% -0.0000 1.0000 0 -5.0000 +% 1.0000 0.0000 0 4.0000 +% 0 0 -1.0000 1.6858 +% 0 0 0 1.0000 + +T = M; +for i = size(thetalist): -1: 1 + T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ForwardDynamics.m b/packages/MATLAB/mr/ForwardDynamics.m new file mode 100644 index 0000000..e24e283 --- /dev/null +++ b/packages/MATLAB/mr/ForwardDynamics.m @@ -0,0 +1,53 @@ +function ddthetalist = ForwardDynamics(thetalist, dthetalist, taulist, ... + g, Ftip, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: A list of joint variables, +% dthetalist: A list of joint rates, +% taulist: An n-vector of joint forces/torques, +% g: Gravity vector g, +% Ftip: Spatial force applied by the end-effector expressed in frame +% {n+1}, +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns, +% Returns ddthetalist: The resulting joint accelerations. +% This function computes ddthetalist by solving: +% Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) ... +% - g(thetalist) - Jtr(thetalist) * Ftip +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% taulist = [0.5; 0.6; 0.7]; +% g = [0; 0; -9.8]; +% Ftip = [1; 1; 1; 1; 1; 1]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% ddthetalist = ForwardDynamics(thetalist, dthetalist, taulist, g, ... +% Ftip, Mlist, Glist, Slist) +% +% Output: +% ddthetalist = +% -0.9739 +% 25.5847 +% -32.9150 + +ddthetalist = MassMatrix(thetalist, Mlist, Glist, Slist) ... + \ (taulist - VelQuadraticForces(thetalist, dthetalist, ... + Mlist, Glist, Slist) ... + - GravityForces(thetalist, g, Mlist, Glist, Slist) ... + - EndEffectorForces(thetalist, Ftip, Mlist, Glist, ... + Slist)); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ForwardDynamicsTrajectory.m b/packages/MATLAB/mr/ForwardDynamicsTrajectory.m new file mode 100644 index 0000000..310d3fa --- /dev/null +++ b/packages/MATLAB/mr/ForwardDynamicsTrajectory.m @@ -0,0 +1,94 @@ +function [thetamat, dthetamat] ... + = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ... + Ftipmat, Mlist, Glist, Slist, dt, ... + intRes) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: n-vector of initial joint variables, +% dthetalist: n-vector of initial joint rates, +% taumat: An N x n matrix of joint forces/torques, where each row is +% the joint effort at any time step, +% g: Gravity vector g, +% Ftipmat: An N x 6 matrix of spatial forces applied by the +% end-effector (If there are no tip forces, the user should +% input a zero and a zero matrix will be used), +% Mlist: List of link frames {i} relative to {i-1} at the home +% position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns, +% dt: The timestep between consecutive joint forces/torques, +% intRes: Integration resolution is the number of times integration +% (Euler) takes places between each time step. Must be an +% integer value greater than or equal to 1. +% Returns thetamat: The N x n matrix of robot joint angles resulting from +% the specified joint forces/torques, +% dthetamat: The N x n matrix of robot joint velocities. +% This function simulates the motion of a serial chain given an open-loop +% history of joint forces/torques. It calls a numerical integration +% procedure that uses ForwardDynamics. +% Example Inputs (3 Link Robot): +% +% clc; clear; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% taumat = [[3.63, -6.58, -5.57]; [3.74, -5.55, -5.5]; ... +% [4.31, -0.68, -5.19]; [5.18, 5.63, -4.31]; ... +% [5.85, 8.17, -2.59]; [5.78, 2.79, -1.7]; ... +% [4.99, -5.3, -1.19]; [4.08, -9.41, 0.07]; ... +% [3.56, -10.1, 0.97]; [3.49, -9.41, 1.23]]; +% %Initialise robot description (Example with 3 links) +% g = [0; 0; -9.8]; +% Ftipmat = ones(size(taumat, 1), 6); +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% dt = 0.1; +% intRes = 8; +% [thetamat, dthetamat] ... +% = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ... +% Ftipmat, Mlist, Glist, Slist, dt, intRes); +% %Output using matplotlib to plot the joint forces/torques +% Tf = size(taumat, 1); +% time=0: (Tf / size(thetamat, 1)): (Tf - (Tf / size(thetamat, 1))); +% plot(time,thetamat(:, 1),'b') +% hold on +% plot(time,thetamat(:, 2), 'g') +% plot(time,thetamat(:, 3), 'r') +% plot(time,dthetamat(:, 1), 'c') +% plot(time,dthetamat(:, 2), 'm') +% plot(time,dthetamat(:, 3), 'y') +% title('Plot of Joint Angles and Joint Velocities') +% xlabel('Time') +% ylabel('Joint Angles/Velocities') +% legend('Theta1', 'Theta2', 'Theta3', 'DTheta1', 'DTheta2', 'DTheta3') +% + +taumat = taumat'; +Ftipmat = Ftipmat'; +thetamat = taumat; +thetamat(:, 1) = thetalist; +dthetamat = taumat; +dthetamat(:, 1) = dthetalist; +for i = 1: size(taumat, 2) - 1 + for j = 1: intRes + ddthetalist ... + = ForwardDynamics(thetalist, dthetalist, taumat(:,i), g, ... + Ftipmat(:, i), Mlist, Glist, Slist); + [thetalist, dthetalist] = EulerStep(thetalist, dthetalist, ... + ddthetalist, dt / intRes); + end + thetamat(:, i + 1) = thetalist; + dthetamat(:, i + 1) = dthetalist; +end +thetamat = thetamat'; +dthetamat = dthetamat'; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/GravityForces.m b/packages/MATLAB/mr/GravityForces.m new file mode 100644 index 0000000..5e52b31 --- /dev/null +++ b/packages/MATLAB/mr/GravityForces.m @@ -0,0 +1,41 @@ +function grav = GravityForces(thetalist, g, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: A list of joint variables, +% g: 3-vector for gravitational acceleration, +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns. +% Returns grav: The joint forces/torques required to overcome gravity at +% thetalist +% This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and +% ddthetalist = 0. +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% g = [0; 0; -9.8]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% grav = GravityForces(thetalist, g, Mlist, Glist, Slist) +% +% Output: +% grav = +% 28.4033 +% -37.6409 +% -5.4416 + +n = size(thetalist, 1); +grav = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1) ,g, ... + [0; 0; 0; 0; 0; 0], Mlist, Glist, Slist); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/IKinBody.m b/packages/MATLAB/mr/IKinBody.m new file mode 100644 index 0000000..8f050ec --- /dev/null +++ b/packages/MATLAB/mr/IKinBody.m @@ -0,0 +1,57 @@ +function [thetalist, success] = IKinBody(Blist, M, T, thetalist0, eomg, ev) +% *** CHAPTER 6: INVERSE KINEMATICS *** +% Takes Blist: The joint screw axes in the end-effector frame when the +% manipulator is at the home position, in the format of a +% matrix with the screw axes as the columns, +% M: The home configuration of the end-effector, +% T: The desired end-effector configuration Tsd, +% thetalist0: An initial guess of joint angles that are close to +% satisfying Tsd, +% eomg: A small positive tolerance on the end-effector orientation +% error. The returned joint angles must give an end-effector +% orientation error less than eomg, +% ev: A small positive tolerance on the end-effector linear position +% error. The returned joint angles must give an end-effector +% position error less than ev. +% Returns thetalist: Joint angles that achieve T within the specified +% tolerances, +% success: A logical value where TRUE means that the function found +% a solution and FALSE means that it ran through the set +% number of maximum iterations without finding a solution +% within the tolerances eomg and ev. +% Uses an iterative Newton-Raphson root-finding method. +% The maximum number of iterations before the algorithm is terminated has +% been hardcoded in as a variable called maxiterations. It is set to 20 at +% the start of the function, but can be changed if needed. +% Example Inputs: +% +% clear; clc; +% Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]]; +% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; +% T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]]; +% thetalist0 = [1.5; 2.5; 3]; +% eomg = 0.01; +% ev = 0.001; +% [thetalist, success] = IKinBody(Blist, M, T, thetalist0, eomg, ev) +% +% Output: +% thetalist = +% 1.5707 +% 2.9997 +% 3.1415 +% success = +% 1 + +thetalist = thetalist0; +i = 0; +maxiterations = 20; +Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T)); +err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev; +while err && i < maxiterations + thetalist = thetalist + pinv(JacobianBody(Blist, thetalist)) * Vb; + i = i + 1; + Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T)); + err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev; +end +success = ~ err; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/IKinSpace.m b/packages/MATLAB/mr/IKinSpace.m new file mode 100644 index 0000000..7ff7385 --- /dev/null +++ b/packages/MATLAB/mr/IKinSpace.m @@ -0,0 +1,62 @@ +function [thetalist, success] ... + = IKinSpace(Slist, M, T, thetalist0, eomg, ev) +% *** CHAPTER 6: INVERSE KINEMATICS *** +% Takes Slist: The joint screw axes in the space frame when the manipulator +% is at the home position, in the format of a matrix with the +% screw axes as the columns, +% M: The home configuration of the end-effector, +% T: The desired end-effector configuration Tsd, +% thetalist0: An initial guess of joint angles that are close to +% satisfying Tsd, +% eomg: A small positive tolerance on the end-effector orientation +% error. The returned joint angles must give an end-effector +% orientation error less than eomg, +% ev: A small positive tolerance on the end-effector linear position +% error. The returned joint angles must give an end-effector +% position error less than ev. +% Returns thetalist: Joint angles that achieve T within the specified +% tolerances, +% success: A logical value where TRUE means that the function found +% a solution and FALSE means that it ran through the set +% number of maximum iterations without finding a solution +% within the tolerances eomg and ev. +% Uses an iterative Newton-Raphson root-finding method. +% The maximum number of iterations before the algorithm is terminated has +% been hardcoded in as a variable called maxiterations. It is set to 20 at +% the start of the function, but can be changed if needed. +% Example Inputs: +% +% clear; clc; +% Slist = [[0; 0; 1; 4; 0; 0], ... +% [0; 0; 0; 0; 1; 0], ... +% [0; 0; -1; -6; 0; -0.1]]; +% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; +% T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]]; +% thetalist0 = [1.5; 2.5; 3]; +% eomg = 0.01; +% ev = 0.001; +% [thetalist, success] = IKinSpace(Slist, M, T, thetalist0, eomg, ev) +% +% Output: +% thetalist = +% 1.5707 +% 2.9997 +% 3.1415 +% success = +% 1 + +thetalist = thetalist0; +i = 0; +maxiterations = 20; +Tsb = FKinSpace(M, Slist, thetalist); +Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T)); +err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev; +while err && i < maxiterations + thetalist = thetalist + pinv(JacobianSpace(Slist, thetalist)) * Vs; + i = i + 1; + Tsb = FKinSpace(M, Slist, thetalist); + Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T)); + err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev; +end +success = ~ err; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/InverseDynamics.m b/packages/MATLAB/mr/InverseDynamics.m new file mode 100644 index 0000000..fe153e3 --- /dev/null +++ b/packages/MATLAB/mr/InverseDynamics.m @@ -0,0 +1,74 @@ +function taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, ... + g, Ftip, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: n-vector of joint variables, +% dthetalist: n-vector of joint rates, +% ddthetalist: n-vector of joint accelerations, +% g: Gravity vector g, +% Ftip: Spatial force applied by the end-effector expressed in frame +% {n+1}, +% Mlist: List of link frames {i} relative to {i-1} at the home +% position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns. +% Returns taulist: The n-vector of required joint forces/torques. +% This function uses forward-backward Newton-Euler iterations to solve the +% equation: +% taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ... +% + g(thetalist) + Jtr(thetalist) * Ftip +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% ddthetalist = [2; 1.5; 1]; +% g = [0; 0; -9.8]; +% Ftip = [1; 1; 1; 1; 1; 1]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ... +% Ftip, Mlist, Glist, Slist) +% +% Output: +% taulist = +% 74.6962 +% -33.0677 +% -3.2306 + +n = size(thetalist, 1); +Mi = eye(4); +Ai = zeros(6, n); +AdTi = zeros(6, 6, n + 1); +Vi = zeros(6, n + 1); +Vdi = zeros(6, n + 1); +Vdi(4: 6, 1) = -g; +AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1))); +Fi = Ftip; +taulist = zeros(n, 1); +for i=1: n + Mi = Mi * Mlist(:, :, i); + Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i); + AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ... + * -thetalist(i))) * TransInv(Mlist(:, :, i))); + Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i); + Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ... + + Ai(:, i) * ddthetalist(i) ... + + ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i); +end +for i = n: -1: 1 + Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ... + - ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1)); + taulist(i) = Fi' * Ai(:, i); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/InverseDynamicsTrajectory.m b/packages/MATLAB/mr/InverseDynamicsTrajectory.m new file mode 100644 index 0000000..3cbd544 --- /dev/null +++ b/packages/MATLAB/mr/InverseDynamicsTrajectory.m @@ -0,0 +1,79 @@ +function taumat ... + = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, ... + g, Ftipmat, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetamat: An N x n matrix of robot joint variables, +% dthetamat: An N x n matrix of robot joint velocities, +% ddthetamat: An N x n matrix of robot joint accelerations, +% g: Gravity vector g, +% Ftipmat: An N x 6 matrix of spatial forces applied by the +% end-effector (If there are no tip forces, the user should +% input a zero and a zero matrix will be used), +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns. +% Returns taumat: The N x n matrix of joint forces/torques for the +% specified trajectory, where each of the N rows is the +% vector of joint forces/torques at each time step. +% This function uses InverseDynamics to calculate the joint forces/torques +% required to move the serial chain along the given trajectory. +% Example Inputs (3 Link Robot) + +% clc; clear; +% %Create a trajectory to follow using functions from Chapter 9 +% thetastart = [0; 0; 0]; +% thetaend = [pi / 2; pi / 2; pi / 2]; +% Tf = 3; +% N= 1000; +% method = 5 ; +% traj = JointTrajectory(thetastart, thetaend, Tf, N, method); +% thetamat = traj; +% dthetamat = zeros(1000, 3); +% ddthetamat = zeros(1000, 3); +% dt = Tf / (N - 1); +% for i = 1: N - 1 +% dthetamat(i + 1, :) = (thetamat(i + 1, :) - thetamat(i, :)) / dt; +% ddthetamat(i + 1, :) = (dthetamat(i + 1, :) - dthetamat(i, :)) / dt; +% end +% %Initialise robot descripstion (Example with 3 links) +% g = [0; 0; -9.8]; +% Ftipmat = ones(N, 6); +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% taumat = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, ... +% g, Ftipmat, Mlist, Glist, Slist); +% %Output using matplotlib to plot the joint forces/torques +% time=0: dt: Tf; +% plot(time, taumat(:, 1), 'b') +% hold on +% plot(time, taumat(:, 2), 'g') +% plot(time, taumat(:, 3), 'r') +% title('Plot for Torque Trajectories') +% xlabel('Time') +% ylabel('Torque') +% legend('Tau1', 'Tau2', 'Tau3') +% + +thetamat = thetamat'; +dthetamat = dthetamat'; +ddthetamat = ddthetamat'; +Ftipmat = Ftipmat'; +taumat = thetamat; +for i = 1: size(thetamat, 2) + taumat(:, i) ... + = InverseDynamics(thetamat(:, i), dthetamat(:, i), ddthetamat(:, i), ... + g, Ftipmat(:, i), Mlist, Glist, Slist); +end +taumat = taumat'; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/JacobianBody.m b/packages/MATLAB/mr/JacobianBody.m new file mode 100644 index 0000000..ea3a924 --- /dev/null +++ b/packages/MATLAB/mr/JacobianBody.m @@ -0,0 +1,33 @@ +function Jb = JacobianBody(Blist, thetalist) +% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** +% Takes Blist: The joint screw axes in the end-effector frame when the +% manipulator is at the home position, in the format of a +% matrix with the screw axes as the columns, +% thetalist: A list of joint coordinates. +% Returns the corresponding body Jacobian (6xn real numbers). +% Example Input: +% +% clear; clc; +% Blist = [[0; 0; 1; 0; 0.2; 0.2], ... +% [1; 0; 0; 2; 0; 3], ... +% [0; 1; 0; 0; 2; 1], ... +% [1; 0; 0; 0.2; 0.3; 0.4]]; +% thetalist = [0.2; 1.1; 0.1; 1.2]; +% Jb = JacobianBody(Blist, thetalist) +% +% Output: +% Jb = +% -0.0453 0.9950 0 1.0000 +% 0.7436 0.0930 0.3624 0 +% -0.6671 0.0362 -0.9320 0 +% 2.3259 1.6681 0.5641 0.2000 +% -1.4432 2.9456 1.4331 0.3000 +% -2.0664 1.8288 -1.5887 0.4000 + +Jb = Blist; +T = eye(4); +for i = length(thetalist) - 1: -1: 1 + T = T * MatrixExp6(VecTose3(-1 * Blist(:, i + 1) * thetalist(i + 1))); + Jb(:, i) = Adjoint(T) * Blist(:, i); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/JacobianSpace.m b/packages/MATLAB/mr/JacobianSpace.m new file mode 100644 index 0000000..a5bd98e --- /dev/null +++ b/packages/MATLAB/mr/JacobianSpace.m @@ -0,0 +1,33 @@ +function Js = JacobianSpace(Slist, thetalist) +% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** +% Takes Slist: The joint screw axes in the space frame when the manipulator +% is at the home position, in the format of a matrix with the +% screw axes as the columns, +% thetalist: A list of joint coordinates. +% Returns the corresponding space Jacobian (6xn real numbers). +% Example Input: +% +% clear; clc; +% Slist = [[0; 0; 1; 0; 0.2; 0.2], ... +% [1; 0; 0; 2; 0; 3], ... +% [0; 1; 0; 0; 2; 1], ... +% [1; 0; 0; 0.2; 0.3; 0.4]]; +% thetalist = [0.2; 1.1; 0.1; 1.2]; +% Js = JacobianSpace(Slist, thetalist) +% +% Output: +% Js = +% 0 0.9801 -0.0901 0.9575 +% 0 0.1987 0.4446 0.2849 +% 1.0000 0 0.8912 -0.0453 +% 0 1.9522 -2.2164 -0.5116 +% 0.2000 0.4365 -2.4371 2.7754 +% 0.2000 2.9603 3.2357 2.2251 + +Js = Slist; +T = eye(4); +for i = 2: length(thetalist) + T = T * MatrixExp6(VecTose3(Slist(:, i - 1) * thetalist(i - 1))); + Js(:, i) = Adjoint(T) * Slist(:, i); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/JointTrajectory.m b/packages/MATLAB/mr/JointTrajectory.m new file mode 100644 index 0000000..9a4157c --- /dev/null +++ b/packages/MATLAB/mr/JointTrajectory.m @@ -0,0 +1,46 @@ +function traj = JointTrajectory(thetastart, thetaend, Tf, N, method) +% *** CHAPTER 9: TRAJECTORY GENERATION *** +% Takes thetastart: The initial joint variables, +% thetaend: The final joint variables, +% Tf: Total time of the motion in seconds from rest to rest, +% N: The number of points N > 1 (Start and stop) in the discrete +% representation of the trajectory, +% method: The time-scaling method, where 3 indicates cubic +% (third-order polynomial) time scaling and 5 indicates +% quintic (fifth-order polynomial) time scaling. +% Returns traj: A trajectory as an N x n matrix, where each row is an +% n-vector of joint variables at an instant in time. The +% first row is thetastart and the Nth row is thetaend . The +% elapsed time between each row is Tf/(N - 1). +% The returned trajectory is a straight-line motion in joint space. +% Example Input: +% +% clear; clc; +% thetastart = [1; 0; 0; 1; 1; 0.2; 0; 1]; +% thetaend = [1.2; 0.5; 0.6; 1.1; 2;2; 0.9; 1]; +% Tf = 4; +% N = 6; +% method = 3; +% traj = JointTrajectory(thetastart, thetaend, Tf, N, method) +% +% Output: +% traj = +% 1.0000 0 0 1.0000 1.0000 0.2000 0 1.0000 +% 1.0208 0.0520 0.0624 1.0104 1.1040 0.3872 0.0936 1.0000 +% 1.0704 0.1760 0.2112 1.0352 1.3520 0.8336 0.3168 1.0000 +% 1.1296 0.3240 0.3888 1.0648 1.6480 1.3664 0.5832 1.0000 +% 1.1792 0.4480 0.5376 1.0896 1.8960 1.8128 0.8064 1.0000 +% 1.2000 0.5000 0.6000 1.1000 2.0000 2.0000 0.9000 1.0000 + +timegap = Tf / (N - 1); +traj = zeros(size(thetastart, 1), N); +for i = 1: N + if method == 3 + s = CubicTimeScaling(Tf, timegap * (i - 1)); + else + s = QuinticTimeScaling(Tf, timegap * (i - 1)); + end + traj(:, i) = thetastart + s * (thetaend - thetastart); +end +traj = traj'; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MassMatrix.m b/packages/MATLAB/mr/MassMatrix.m new file mode 100644 index 0000000..4ec3ce3 --- /dev/null +++ b/packages/MATLAB/mr/MassMatrix.m @@ -0,0 +1,47 @@ +function M = MassMatrix(thetalist, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: A list of joint variables, +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns. +% Returns M: The numerical inertia matrix M(thetalist) of an n-joint serial +% chain at the given configuration thetalist. +% This function calls InverseDynamics n times, each time passing a +% ddthetalist vector with a single element equal to one and all other +% inputs set to zero. Each call of InverseDynamics generates a single +% column, and these columns are assembled to create the inertia matrix. +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% M = MassMatrix(thetalist, Mlist, Glist, Slist) +% +% Output: +% M = +% 22.5433 -0.3071 -0.0072 +% -0.3071 1.9685 0.4322 +% -0.0072 0.4322 0.1916 + +n = size(thetalist, 1); +M = zeros(n); +for i = 1: n + ddthetalist = zeros(n, 1); + ddthetalist(i) = 1; + M(:, i) = InverseDynamics(thetalist, zeros(n, 1), ddthetalist, ... + [0; 0; 0], [0; 0; 0; 0; 0; 0],Mlist, ... + Glist, Slist); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MatrixExp3.m b/packages/MATLAB/mr/MatrixExp3.m new file mode 100644 index 0000000..ce60f20 --- /dev/null +++ b/packages/MATLAB/mr/MatrixExp3.m @@ -0,0 +1,26 @@ +function R = MatrixExp3(so3mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 3x3 so(3) representation of exponential coordinates. +% Returns R in SO(3) that is achieved by rotating about omghat by theta +% from an initial orientation R = I. +% Example Input: +% +% clear; clc; +% so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]]; +% R = MatrixExp3(so3mat) +% +% Output: +% R = +% -0.6949 0.7135 0.0893 +% -0.1920 -0.3038 0.9332 +% 0.6930 0.6313 0.3481 + +omgtheta = so3ToVec(so3mat); +if NearZero(norm(omgtheta)) + R = eye(3); +else + [omghat, theta] = AxisAng3(omgtheta); + omgmat = so3mat / theta; + R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MatrixExp6.m b/packages/MATLAB/mr/MatrixExp6.m new file mode 100644 index 0000000..24b0173 --- /dev/null +++ b/packages/MATLAB/mr/MatrixExp6.m @@ -0,0 +1,34 @@ +function T = MatrixExp6(se3mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a se(3) representation of exponential coordinates. +% Returns a T matrix in SE(3) that is achieved by traveling along/about the +% screw axis S for a distance theta from an initial configuration T = I. +% Example Input: +% +% clear; clc; +% se3mat = [ 0, 0, 0, 0; +% 0, 0, -1.5708, 2.3562; +% 0, 1.5708, 0, 2.3562; +% 0, 0, 0, 0] +% T = MatrixExp6(se3mat) +% +% Output: +% T = +% 1.0000 0 0 0 +% 0 0.0000 -1.0000 -0.0000 +% 0 1.0000 0.0000 3.0000 +% 0 0 0 1.0000 + +omgtheta = so3ToVec(se3mat(1: 3, 1: 3)); +if NearZero(norm(omgtheta)) + T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1]; +else + [omghat, theta] = AxisAng3(omgtheta); + omgmat = se3mat(1: 3, 1: 3) / theta; + T = [MatrixExp3(se3mat(1: 3, 1: 3)), ... + (eye(3) * theta + (1 - cos(theta)) * omgmat ... + + (theta - sin(theta)) * omgmat * omgmat) ... + * se3mat(1: 3, 4) / theta; + 0, 0, 0, 1]; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MatrixLog3.m b/packages/MATLAB/mr/MatrixLog3.m new file mode 100644 index 0000000..bdb41d9 --- /dev/null +++ b/packages/MATLAB/mr/MatrixLog3.m @@ -0,0 +1,37 @@ +function so3mat = MatrixLog3(R) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes R (rotation matrix). +% Returns the corresponding so(3) representation of exponential +% coordinates. +% Example Input: +% +% clear; clc; +% R = [[0, 0, 1]; [1, 0, 0]; [0, 1, 0]]; +% so3mat = MatrixLog3(R) +% +% Output: +% angvmat = +% 0 -1.2092 1.2092 +% 1.2092 0 -1.2092 +% -1.2092 1.2092 0 + +acosinput = (trace(R) - 1) / 2; +if acosinput >= 1 + so3mat = zeros(3); +elseif acosinput <= -1 + if ~NearZero(1 + R(3, 3)) + omg = (1 / sqrt(2 * (1 + R(3, 3)))) ... + * [R(1, 3); R(2, 3); 1 + R(3, 3)]; + elseif ~NearZero(1 + R(2, 2)) + omg = (1 / sqrt(2 * (1 + R(2, 2)))) ... + * [R(1, 2); 1 + R(2, 2); R(3, 2)]; + else + omg = (1 / sqrt(2 * (1 + R(1, 1)))) ... + * [1 + R(1, 1); R(2, 1); R(3, 1)]; + end + so3mat = VecToso3(pi * omg); +else + theta = acos(acosinput); + so3mat = theta * (1 / (2 * sin(theta))) * (R - R'); +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MatrixLog6.m b/packages/MATLAB/mr/MatrixLog6.m new file mode 100644 index 0000000..f717eab --- /dev/null +++ b/packages/MATLAB/mr/MatrixLog6.m @@ -0,0 +1,30 @@ +function expmat = MatrixLog6(T) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a transformation matrix T in SE(3). +% Returns the corresponding se(3) representation of exponential +% coordinates. +% Example Input: +% +% clear; clc; +% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; +% expmat = MatrixLog6(T) +% +% Output: +% expc6 = +% 0 0 0 0 +% 0 0 -1.5708 2.3562 +% 0 1.5708 0 2.3562 +% 0 0 0 0 + +[R, p] = TransToRp(T); +omgmat = MatrixLog3(R); +if isequal(omgmat, zeros(3)) + expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0]; +else + theta = acos((trace(R) - 1) / 2); + expmat = [omgmat, (eye(3) - omgmat / 2 ... + + (1 / theta - cot(theta / 2) / 2) ... + * omgmat * omgmat / theta) * p; + 0, 0, 0, 0]; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/NearZero.m b/packages/MATLAB/mr/NearZero.m new file mode 100644 index 0000000..d5abd99 --- /dev/null +++ b/packages/MATLAB/mr/NearZero.m @@ -0,0 +1,16 @@ +function judge = NearZero(near) +% *** BASIC HELPER FUNCTIONS *** +% Takes a scalar. +% Checks if the scalar is small enough to be neglected. +% Example Input: +% +% clear; clc; +% near = -1e-7; +% judge = NearZero(near) +% +% Output: +% judge = +% 1 + +judge = norm(near) < 1e-6; +end diff --git a/packages/MATLAB/mr/Normalize.m b/packages/MATLAB/mr/Normalize.m new file mode 100644 index 0000000..9555187 --- /dev/null +++ b/packages/MATLAB/mr/Normalize.m @@ -0,0 +1,19 @@ +function norm_v = Normalize(V) +% *** BASIC HELPER FUNCTIONS *** +% Takes in a vector. +% Scales it to a unit vector. +% Example Input: +% +% clear; clc; +% V = [1; 2; 3]; +% norm_v = Normalize(V) +% +% Output: +% norm_v = +% 0.2673 +% 0.5345 +% 0.8018 + +norm_v = V / norm(V); +end + diff --git a/packages/MATLAB/mr/ProjectToSE3.m b/packages/MATLAB/mr/ProjectToSE3.m new file mode 100644 index 0000000..9e67618 --- /dev/null +++ b/packages/MATLAB/mr/ProjectToSE3.m @@ -0,0 +1,25 @@ +function T = ProjectToSE3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A matrix near SE(3) to project to SE(3). +% Returns T representing the closest rotation matrix that is in SE(3). +% This function uses singular-value decomposition (see +% http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review) +% and is only appropriate for matrices close to SE(3). +% Example Inputs: +% +% clear; clc; +% mat = [ 0.675, 0.150, 0.720, 1.2; +% 0.370, 0.771, -0.511, 5.4; +% -0.630, 0.619, 0.472, 3.6; +% 0.003, 0.002, 0.010, 0.9]; +% T = ProjectToSE3(mat) +% +% Output: +% T = +% 0.6790 0.1489 0.7189 1.2000 +% 0.3732 0.7732 -0.5127 5.4000 +% -0.6322 0.6164 0.4694 3.6000 +% 0 0 0 1.0000 + +T = RpToTrans(ProjectToSO3(mat(1: 3, 1: 3)), mat(1: 3, 4)); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ProjectToSO3.m b/packages/MATLAB/mr/ProjectToSO3.m new file mode 100644 index 0000000..93991f7 --- /dev/null +++ b/packages/MATLAB/mr/ProjectToSO3.m @@ -0,0 +1,28 @@ +function R = ProjectToSO3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A matrix near SO(3) to project to SO(3). +% Returns R representing the closest rotation matrix that is in SO(3). +% This function uses singular-value decomposition (see +% http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review) +% and is only appropriate for matrices close to SO(3). +% Example Inputs: +% +% clear; clc; +% mat = [ 0.675, 0.150, 0.720; +% 0.370, 0.771, -0.511; +% -0.630, 0.619, 0.472]; +% R = ProjectToSO3(mat) +% +% Output: +% R = +% 0.6790 0.1489 0.7189 +% 0.3732 0.7732 -0.5127 +% -0.6322 0.6164 0.4694 + +[U, S, V] = svd(mat); +R = U * V'; +if det(R) < 0 + % In this case the result may be far from mat. + R = [R(:, 1: 2), -R(:, 3)]; +end +end diff --git a/packages/MATLAB/mr/QuinticTimeScaling.m b/packages/MATLAB/mr/QuinticTimeScaling.m new file mode 100644 index 0000000..2823c93 --- /dev/null +++ b/packages/MATLAB/mr/QuinticTimeScaling.m @@ -0,0 +1,20 @@ +function s = QuinticTimeScaling(Tf, t) +% *** CHAPTER 9: TRAJECTORY GENERATION *** +% Takes Tf: Total time of the motion in seconds from rest to rest, +% t: The current time t satisfying 0 < t < Tf. +% Returns s: The path parameter s(t) corresponding to a fifth-order +% polynomial motion that begins and ends at zero velocity and +% zero acceleration. +% Example Input: +% +% clear; clc; +% Tf = 2; +% t = 0.6; +% s = QuinticTimeScaling(Tf,t) +% +% Output: +% s = +% 0.1631 + +s = 10 * (t / Tf) ^ 3 - 15 * (t / Tf) ^ 4 + 6 * (t / Tf) ^ 5; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/RotInv.m b/packages/MATLAB/mr/RotInv.m new file mode 100644 index 0000000..9e07a66 --- /dev/null +++ b/packages/MATLAB/mr/RotInv.m @@ -0,0 +1,18 @@ +function invR = RotInv(R) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 3x3 rotation matrix. +% Returns the inverse (transpose). +% Example Input: +% +% clear; clc; +% R = [0, 0, 1; 1, 0, 0; 0, 1, 0]; +% invR = RotInv(R) +% +% Output: +% invR = +% 0 1 0 +% 0 0 1 +% 1 0 0 + +invR = transpose(R); +end diff --git a/packages/MATLAB/mr/RpToTrans.m b/packages/MATLAB/mr/RpToTrans.m new file mode 100644 index 0000000..4d3017a --- /dev/null +++ b/packages/MATLAB/mr/RpToTrans.m @@ -0,0 +1,20 @@ +function T = RpToTrans(R, p) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes rotation matrix R and position p. +% Returns the corresponding homogeneous transformation matrix T in SE(3). +% Example Input: +% +% clear; clc; +% R = [[1, 0, 0]; [0, 0, -1]; [0, 1, 0]]; +% p = [1; 2; 5]; +% T = RpToTrans(R, p) +% +% Output: +% T = +% 1 0 0 1 +% 0 0 -1 2 +% 0 1 0 5 +% 0 0 0 1 + +T = [R, p; 0, 0, 0, 1]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ScrewToAxis.m b/packages/MATLAB/mr/ScrewToAxis.m new file mode 100644 index 0000000..fd4d7ce --- /dev/null +++ b/packages/MATLAB/mr/ScrewToAxis.m @@ -0,0 +1,25 @@ +function S = ScrewToAxis(q, s, h) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes q: a point lying on the screw axis, +% s: a unit vector in the direction of the screw axis, +% h: the pitch of the screw axis. +% Returns the corresponding normalized screw axis. +% Example Input: +% +% clear; clc; +% q = [3; 0; 0]; +% s = [0; 0; 1]; +% h = 2; +% S = ScrewToAxis(q, s, h) +% +% Output: +% S = +% 0 +% 0 +% 1 +% 0 +% -3 +% 2 + +S = [s; cross(q, s) + h * s]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ScrewTrajectory.m b/packages/MATLAB/mr/ScrewTrajectory.m new file mode 100644 index 0000000..eb527ca --- /dev/null +++ b/packages/MATLAB/mr/ScrewTrajectory.m @@ -0,0 +1,59 @@ +function traj = ScrewTrajectory(Xstart, Xend, Tf, N, method) +% *** CHAPTER 9: TRAJECTORY GENERATION *** +% Takes Xstart: The initial end-effector configuration, +% Xend: The final end-effector configuration, +% Tf: Total time of the motion in seconds from rest to rest, +% N: The number of points N > 1 (Start and stop) in the discrete +% representation of the trajectory, +% method: The time-scaling method, where 3 indicates cubic +% (third-order polynomial) time scaling and 5 indicates +% quintic (fifth-order polynomial) time scaling. +% Returns traj: The discretized trajectory as a list of N matrices in SE(3) +% separated in time by Tf/(N-1). The first in the list is +% Xstart and the Nth is Xend . +% This function calculates a trajectory corresponding to the screw motion +% about a space screw axis. +% Example Input: +% +% clear; clc; +% Xstart = [[1 ,0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]]; +% Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]]; +% Tf = 5; +% N = 4; +% method = 3; +% traj = ScrewTrajectory(Xstart, Xend, Tf, N, method) +% +% Output: +% traj = +% 1.0000 0 0 1.0000 +% 0 1.0000 0 0 +% 0 0 1.0000 1.0000 +% 0 0 0 1.0000 +% +% 0.9041 -0.2504 0.3463 0.4410 +% 0.3463 0.9041 -0.2504 0.5287 +% -0.2504 0.3463 0.9041 1.6007 +% 0 0 0 1.0000 +% +% 0.3463 -0.2504 0.9041 -0.1171 +% 0.9041 0.3463 -0.2504 0.4727 +% -0.2504 0.9041 0.3463 3.2740 +% 0 0 0 1.0000 +% +% -0.0000 0.0000 1.0000 0.1000 +% 1.0000 -0.0000 0.0000 -0.0000 +% 0.0000 1.0000 -0.0000 4.1000 +% 0 0 0 1.0000 + +timegap = Tf / (N - 1); +traj = cell(1, N); +for i = 1: N + if method == 3 + s = CubicTimeScaling(Tf, timegap * (i - 1)); + else + s = QuinticTimeScaling(Tf, timegap * (i - 1)); + end + traj{i} = Xstart * MatrixExp6(MatrixLog6(TransInv(Xstart) * Xend) * s); +end +end + diff --git a/packages/MATLAB/mr/SimulateControl.m b/packages/MATLAB/mr/SimulateControl.m new file mode 100644 index 0000000..4c3ca3f --- /dev/null +++ b/packages/MATLAB/mr/SimulateControl.m @@ -0,0 +1,143 @@ +function [taumat, thetamat] ... + = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, ... + Glist, Slist, thetamatd, dthetamatd, ... + ddthetamatd, gtilde, Mtildelist, Gtildelist, ... + Kp, Ki, Kd, dt, intRes) +% *** CHAPTER 11: ROBOT CONTROL *** +% Takes thetalist: n-vector of initial joint variables, +% dthetalist: n-vector of initial joint velocities, +% g: Actual gravity vector g, +% Ftipmat: An N x 6 matrix of spatial forces applied by the +% end-effector (If there are no tip forces, the user should +% input a zero and a zero matrix will be used), +% Mlist: Actual list of link frames i relative to i? at the home +% position, +% Glist: Actual spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns, +% thetamatd: An Nxn matrix of desired joint variables from the +% reference trajectory, +% dthetamatd: An Nxn matrix of desired joint velocities, +% ddthetamatd: An Nxn matrix of desired joint accelerations, +% gtilde: The gravity vector based on the model of the actual robot +% (actual values given above), +% Mtildelist: The link frame locations based on the model of the +% actual robot (actual values given above), +% Gtildelist: The link spatial inertias based on the model of the +% actual robot (actual values given above), +% Kp: The feedback proportional gain (identical for each joint), +% Ki: The feedback integral gain (identical for each joint), +% Kd: The feedback derivative gain (identical for each joint), +% dt: The timestep between points on the reference trajectory. +% intRes: Integration resolution is the number of times integration +% (Euler) takes places between each time step. Must be an +% integer value greater than or equal to 1. +% Returns taumat: An Nxn matrix of the controller commanded joint +% forces/torques, where each row of n forces/torques +% corresponds to a single time instant, +% thetamat: An Nxn matrix of actual joint angles. +% The end of this function plots all the actual and desired joint angles. +% Example Usage +% +% clc; clear; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% %Initialize robot description (Example with 3 links) +% g = [0; 0; -9.8]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% dt = 0.01; +% %Create a trajectory to follow +% thetaend =[pi / 2; pi; 1.5 * pi]; +% Tf = 1; +% N = Tf / dt; +% method = 5; +% thetamatd = JointTrajectory(thetalist, thetaend, Tf, N, method); +% dthetamatd = zeros(N, 3); +% ddthetamatd = zeros(N, 3); +% dt = Tf / (N - 1); +% for i = 1: N - 1 +% dthetamatd(i + 1, :) = (thetamatd(i + 1, :) - thetamatd(i, :)) / dt; +% ddthetamatd(i + 1, :) = (dthetamatd(i + 1, :) ... +% - dthetamatd(i, :)) / dt; +% end +% %Possibly wrong robot description (Example with 3 links) +% gtilde = [0.8; 0.2; -8.8]; +% Mhat01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.1]; [0, 0, 0, 1]]; +% Mhat12 = [[0, 0, 1, 0.3]; [0, 1, 0, 0.2]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% Mhat23 = [[1, 0, 0, 0]; [0, 1, 0, -0.2]; [0, 0, 1, 0.4]; [0, 0, 0, 1]]; +% Mhat34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.2]; [0, 0, 0, 1]]; +% Ghat1 = diag([0.1, 0.1, 0.1, 4, 4, 4]); +% Ghat2 = diag([0.3, 0.3, 0.1, 9, 9, 9]); +% Ghat3 = diag([0.1, 0.1, 0.1, 3, 3, 3]); +% Gtildelist = cat(3, Ghat1, Ghat2, Ghat3); +% Mtildelist = cat(4, Mhat01, Mhat12, Mhat23, Mhat34); +% Ftipmat = ones(N, 6); +% Kp = 20; +% Ki = 10; +% Kd = 18; +% intRes = 8; +% [taumat, thetamat] ... +% = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, ... +% Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, ... +% Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes); +% + +Ftipmat = Ftipmat'; +thetamatd = thetamatd'; +dthetamatd = dthetamatd'; +ddthetamatd = ddthetamatd'; +n = size(thetamatd, 2); +taumat = zeros(size(thetamatd)); +thetamat = zeros(size(thetamatd)); +thetacurrent = thetalist; +dthetacurrent = dthetalist; +eint = zeros(size(thetamatd, 1), 1); +for i=1: n + taulist ... + = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, ... + Mtildelist, Gtildelist, Slist, thetamatd(:, i), ... + dthetamatd(:, i), ddthetamatd(:, i), Kp, Ki, Kd); + for j=1: intRes + ddthetalist ... + = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, ... + Ftipmat(:, i), Mlist, Glist, Slist); + [thetacurrent, dthetacurrent] ... + = EulerStep(thetacurrent, dthetacurrent, ddthetalist, ... + (dt / intRes)); + end + taumat(:, i) = taulist; + thetamat(:, i) = thetacurrent; + eint = eint + (dt*(thetamatd(:, i) - thetacurrent)); +end +%Output using matplotlib +links = size(thetamat, 1); +leg = cell(1, 2 * links); +time=0: dt: dt * n - dt; +timed=0: dt: dt * n - dt; +figure +hold on +for i=1: links + col = rand(1, 3); + plot(time, (thetamat(i, :)'), '-', 'Color', col) + plot(timed, (thetamatd(i, :)'), '.', 'Color', col) + leg{2 * i - 1} = (strcat('ActualTheta', num2str(i))); + leg{2 * i} = (strcat('DesiredTheta', num2str(i))); +end +title('Plot of Actual and Desired Joint Angles') +xlabel('Time') +ylabel('Joint Angles') +legend(leg, 'Location', 'NorthWest') +taumat = taumat'; +thetamat = thetamat'; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/TestIfSE3.m b/packages/MATLAB/mr/TestIfSE3.m new file mode 100644 index 0000000..deeed8c --- /dev/null +++ b/packages/MATLAB/mr/TestIfSE3.m @@ -0,0 +1,19 @@ +function judge = TestIfSE3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A 4x4 matrix. +% Check if mat is close to or on the manifold SE(3). +% Example Inputs: +% +% clear; clc; +% mat = [1.0, 0.0, 0.0, 1.2; +% 0.0, 0.1, -0.95, 1.5; +% 0.0, 1.0, 0.1, -0.9; +% 0.0, 0.0, 0.1, 0.98]; +% judge = TestIfSE3(mat) +% +% Output: +% judge = +% 0 + +judge = norm(DistanceToSE3(mat)) < 1e-3; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/TestIfSO3.m b/packages/MATLAB/mr/TestIfSO3.m new file mode 100644 index 0000000..2ba917a --- /dev/null +++ b/packages/MATLAB/mr/TestIfSO3.m @@ -0,0 +1,18 @@ +function judge = TestIfSO3(mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes mat: A 3x3 matrix. +% Check if mat is close to or on the manifold SO(3). +% Example Inputs: +% +% clear; clc; +% mat = [1.0, 0.0, 0.0; +% 0.0, 0.1, -0.95; +% 0.0, 1.0, 0.1]; +% judge = TestIfSO3(mat) +% +% Output: +% dudge = +% 0 + +judge = norm(DistanceToSO3(mat)) < 1e-3; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/TransInv.m b/packages/MATLAB/mr/TransInv.m new file mode 100644 index 0000000..d153a60 --- /dev/null +++ b/packages/MATLAB/mr/TransInv.m @@ -0,0 +1,22 @@ +function invT = TransInv(T) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a transformation matrix T. +% Returns its inverse. +% Uses the structure of transformation matrices to avoid taking a matrix +% inverse, for efficiency. +% Example Input: +% +% clear; clc; +% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; +% invT = TransInv(T) +% +% Ouput: +% invT = +% 1 0 0 0 +% 0 0 1 -3 +% 0 -1 0 0 +% 0 0 0 1 + +[R, p] = TransToRp(T); +invT = [transpose(R), -transpose(R) * p; 0, 0, 0, 1]; +end diff --git a/packages/MATLAB/mr/TransToRp.m b/packages/MATLAB/mr/TransToRp.m new file mode 100644 index 0000000..d4daac2 --- /dev/null +++ b/packages/MATLAB/mr/TransToRp.m @@ -0,0 +1,24 @@ +function [R, p] = TransToRp(T) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes the transformation matrix T in SE(3) +% Returns R: the corresponding rotation matrix +% p: the corresponding position vector . +% Example Input: +% +% clear; clc; +% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; +% [R, p] = TransToRp(T) +% +% Output: +% R = +% 1 0 0 +% 0 0 -1 +% 0 1 0 +% p = +% 0 +% 0 +% 3 + +R = T(1: 3, 1: 3); +p = T(1: 3, 4); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/VecTose3.m b/packages/MATLAB/mr/VecTose3.m new file mode 100644 index 0000000..7d74eeb --- /dev/null +++ b/packages/MATLAB/mr/VecTose3.m @@ -0,0 +1,19 @@ +function se3mat = VecTose3(V) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 6-vector (representing a spatial velocity). +% Returns the corresponding 4x4 se(3) matrix. +% Example Input: +% +% clear; clc; +% V = [1; 2; 3; 4; 5; 6]; +% se3mat = VecTose3(V) +% +% Output: +% se3mat = +% 0 -3 2 4 +% 3 0 -1 5 +% -2 1 0 6 +% 0 0 0 0 + +se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/VecToso3.m b/packages/MATLAB/mr/VecToso3.m new file mode 100644 index 0000000..4ce79b4 --- /dev/null +++ b/packages/MATLAB/mr/VecToso3.m @@ -0,0 +1,18 @@ +function so3mat = VecToso3(omg) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 3-vector (angular velocity). +% Returns the skew symmetric matrix in so(3). +% Example Input: +% +% clear; clc; +% omg = [1; 2; 3]; +% so3mat = VecToso3(omg) +% +% Output: +% so3mat = +% 0 -3 2 +% 3 0 -1 +% -2 1 0 + +so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/VelQuadraticForces.m b/packages/MATLAB/mr/VelQuadraticForces.m new file mode 100644 index 0000000..f02b15c --- /dev/null +++ b/packages/MATLAB/mr/VelQuadraticForces.m @@ -0,0 +1,41 @@ +function c = VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes thetalist: A list of joint variables, +% dthetalist: A list of joint rates, +% Mlist: List of link frames i relative to i-1 at the home position, +% Glist: Spatial inertia matrices Gi of the links, +% Slist: Screw axes Si of the joints in a space frame, in the format +% of a matrix with the screw axes as the columns, +% Returns c: The vector c(thetalist,dthetalist) of Coriolis and centripetal +% terms for a given thetalist and dthetalist. +% This function calls InverseDynamics with g = 0, Ftip = 0, and +% ddthetalist = 0. +% Example Input (3 Link Robot): +% +% clear; clc; +% thetalist = [0.1; 0.1; 0.1]; +% dthetalist = [0.1; 0.2; 0.3]; +% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; +% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; +% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; +% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; +% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); +% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); +% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); +% Glist = cat(3, G1, G2, G3); +% Mlist = cat(3, M01, M12, M23, M34); +% Slist = [[1; 0; 1; 0; 1; 0], ... +% [0; 1; 0; -0.089; 0; 0], ... +% [0; 1; 0; -0.089; 0; 0.425]]; +% c = VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist) +% +% Output: +% c = +% 0.2645 +% -0.0551 +% -0.0069 + +c = InverseDynamics(thetalist, dthetalist, ... + zeros(size(thetalist, 1), 1), [0; 0; 0], ... + [0; 0; 0; 0; 0; 0], Mlist, Glist, Slist); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/ad.m b/packages/MATLAB/mr/ad.m new file mode 100644 index 0000000..abd4ed0 --- /dev/null +++ b/packages/MATLAB/mr/ad.m @@ -0,0 +1,23 @@ +function adV = ad(V) +% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +% Takes V: 6-vector spatial velocity. +% Returns adV: The corresponding 6x6 matrix. +% Used to calculate the Lie bracket [V1, V2] = [adV1]V2 +% Example Input: +% +% clear; clc; +% V = [1; 2; 3; 4; 5; 6]; +% adV = ad(V) +% +% Output: +% adV = +% 0 -3 2 0 0 0 +% 3 0 -1 0 0 0 +% -2 1 0 0 0 0 +% 0 -6 5 0 -3 2 +% 6 0 -4 3 0 -1 +% -5 4 0 -2 1 0 + +omgmat = VecToso3(V(1: 3)); +adV = [omgmat, zeros(3); VecToso3(V(4: 6)), omgmat]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/se3ToVec.m b/packages/MATLAB/mr/se3ToVec.m new file mode 100644 index 0000000..990f7f6 --- /dev/null +++ b/packages/MATLAB/mr/se3ToVec.m @@ -0,0 +1,21 @@ +function V = se3ToVec(se3mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes se3mat a 4x4 se(3) matrix +% Returns the corresponding 6-vector (representing spatial velocity). +% Example Input: +% +% clear; clc; +% se3mat = [[0, -3, 2, 4]; [3, 0, -1, 5]; [-2, 1, 0, 6]; [0, 0, 0, 0]]; +% V = se3ToVec(se3mat) +% +% Output: +% V = +% 1 +% 2 +% 3 +% 4 +% 5 +% 6 + +V = [se3mat(3, 2); se3mat(1, 3); se3mat(2, 1); se3mat(1: 3, 4)]; +end \ No newline at end of file diff --git a/packages/MATLAB/mr/so3ToVec.m b/packages/MATLAB/mr/so3ToVec.m new file mode 100644 index 0000000..21b9b16 --- /dev/null +++ b/packages/MATLAB/mr/so3ToVec.m @@ -0,0 +1,18 @@ +function omg = so3ToVec(so3mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a 3x3 skew-symmetric matrix (an element of so(3)). +% Returns the corresponding 3-vector (angular velocity). +% Example Input: +% +% clear; clc; +% so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]]; +% omg = so3ToVec(so3mat) +% +% Output: +% omg = +% 1 +% 2 +% 3 + +omg = [so3mat(3, 2); so3mat(1, 3); so3mat(2, 1)]; +end \ No newline at end of file diff --git a/packages/Mathematica/ModernRobotics.m b/packages/Mathematica/ModernRobotics.m new file mode 100644 index 0000000..9c9615f --- /dev/null +++ b/packages/Mathematica/ModernRobotics.m @@ -0,0 +1,1830 @@ +(* ::Package:: *) + +(* ::Title::Closed:: *) +(*Modern Robotics: Mechanics, Planning, and Control*) +(*Code Library*) + + +(* ::Text:: *) +(* :Name: ModernRobotics` *) +(* :Book: Modern Robotics: Mechanics, Planning, and Control *) +(* :Author: Huan Weng, Jarvis Schultz, Mikhail Todes*) +(* :Contact: huanweng@u.northwestern.edu*) +(* :Summary: This package is the code library accompanying the book. *) +(* :Package Version: 1.1.0*) +(* :Mathematica Version: 11.3 *) +(* Tested in Mathematica 11.3 *) + + +BeginPackage["ModernRobotics`"]; + + +(* ::Section::Closed:: *) +(*USAGE*) + + +(* ::Subsection::Closed:: *) +(*BASIC HELPER FUNCTIONS*) + + +NearZero::usage= +"NearZero[s]: +Takes a scalar. +Checks if the scalar is small enough to be neglected. + +Example: + Input: + judge = NearZero[-10^-7] + Output: + True" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 3: RIGID-BODY MOTION*) + + +RotInv::usage= +"RotInv[R]: +Takes a 3x3 rotation matrix. +Returns the inverse (transpose). + +Example: + Input: + invR = RotInv[{{0,0,1},{1,0,0},{0,1,0}}] + Output: + {{0,1,0},{0,0,1},{1,0,0}}" + + +VecToso3::usage= +"VecToso3[omg]: +Takes a 3-vector (angular velocity). +Returns the skew symmetric matrix in so3. + +Example: + Input: + so3mat = VecToso3[{{1},{2},{3}}] + Output: + {{0,-3,2},{3,0,-1},{-2,1,0}}" + + +so3ToVec::usage= +"so3ToVec[so3mat]: +Takes a 3x3 skew-symmetric matrix (an element of so(3)). +Returns the corresponding vector (angular velocity). + +Example: + Input: + omg = so3ToVec[{{0,-3,2},{3,0,-1},{-2,1,0}}] + Output: + {{1},{2},{3}}" + + +AxisAng3::usage= +"AxisAng3[expc3]: + Takes A 3-vector of exponential coordinates for rotation. + Returns unit rotation axis omghat and the corresponding rotation angle + theta. + The first element of the output is the unit rotation axis column vector + (omghat) and the second element is theta. + +Example: + Input: + {omghat,theta} = AxisAng3[{{1,2,3}}\[Transpose]]//N + Output: + {{{0.2672612419124244},{0.5345224838248488},{0.8017837257372732}}, + 3.7416573867739413}" + + +MatrixExp3::usage= +"MatrixExp3[so3mat]: +Takes a so(3) representation of exponential coordinates. +Returns R in SO(3) that is achieved by rotating about omghat by theta from an +initial orientation R=I. + +Example: + Input: + R = MatrixExp3[{{0,-3,2},{3,0,-1},{-2,1,0}}]//N + Output: + {{-0.694921,0.713521,0.0892929},{-0.192007,-0.303785,0.933192}, + {0.692978,0.63135,0.348107}}" + + +MatrixLog3::usage= +"MatrixLog3[R]: +Takes R (rotation matrix). +Returns the corresponding so(3) representation of exponential coordinates. + +Example: + Input: + so3mat = MatrixLog3[{{0,0,1},{1,0,0},{0,1,0}}]//N + Output: + {{0,-1.2092,1.2092},{1.2092,0,-1.2092},{-1.2092,1.2092,0}}" + + +DistanceToSO3::usage= +"DistanceToSO3[mat]: +Takes mat as a 3x3 matrix. +Returns the Frobenius norm to describe the distance of mat from the SO(3) +manifold. +Computes the distance from mat to the SO(3) manifold using the following +method: If det(mat) <= 0, return a large number. If det(mat) > 0, return +norm(mat'.mat - I). + +Example: + Input: + d = DistanceToSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}] + Output: + 0.088353" + + +TestIfSO3::usage= +"TestIfSO3[mat]: +Takes mat as a 3x3 matrix. +Check if mat is close to or on the manifold SO(3). + +Example: + Input: + judge = TestIfSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}] + Output: + False" + + +ProjectToSO3::usage= +"ProjectToSO3[mat]: +Takes a matrix near SO(3) to project to SO(3). +Returns the closest rotation matrix that is in SO(3). +This function uses singular-value decomposition (see +http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review) +and is only appropriate for matrices close to SO(3). + +Example: + Input: + R = ProjectToSO3[{{0.675,0.150,0.720},{0.370,0.771,-0.511}, + {-0.630,0.619,0.472}}]//N + Output: + {{0.679011,0.148945,0.718859},{0.373207,0.773196,-0.512723}, + {-0.632187,0.616428,0.469421}}" + + +RpToTrans::usage= +"RpToTrans[R,p]: +Takes rotation matrix R and position p. +Returns corresponding homogeneous transformation matrix T in SE(3). + +Example: + Input: + T = RpToTrans[{{1,0,0},{0,0,-1},{0,1,0}},{{1,2,5}}\[Transpose]] + Output: + {{1, 0, 0, 1}, {0, 0, -1, 2}, {0, 1, 0, 5}, {0, 0, 0, 1}}" + + +TransToRp::usage= +"TransToRp[T]: +Takes transformation matrix T in SE(3). +Returns + R: the corresponding rotation matrix, + p: the corresponding position vector. +The first element of the output is the rotation matrix and the second is the +position vector. + +Example: + Input: + {R,p} = TransToRp[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}] + Output: + {{{1,0,0},{0,0,-1},{0,1,0}},{{0},{0},{3}}}" + + +TransInv::usage= +"TransInv[T]: +Takes T a transformation matrix. +Returns its inverse. +Used the structure of transformation matrices to avoid taking a matrix +inverse, for efficiency. + +Example: + Input: + invT = TransInv[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}] + Output: + {{1,0,0,0},{0,0,1,-3}, {0,-1,0,0},{0,0,0,1}}" + + +VecTose3::usage= +"VecTose3[V]: +Takes a 6-vector (representing a spatial velocity). +Returns the corresponding 4x4 se(3) matrix + +Example: + Input: + se3mat = VecTose3[{{1,2,3,4,5,6}}\[Transpose]] + Output: + {{0,-3,2,4},{3,0,-1,5},{-2,1,0,6},{0,0,0,0}}" + + +se3ToVec::usage= +"se3ToVec[se3mat]: +Takes se3mat a 4x4 se(3) matrix. +Returns the corresponding 6-vector (representing spatial velocity). + +Example: + Input: + V = se3ToVec[{{0,-3,2,4},{3,0,-1,5},{-2,1,0,6},{0,0,0,0}}] + Output: + {{1},{2},{3},{4},{5},{6}}" + + +Adjoint::usage= +"Adjoint[T]: + Takes T a transformation matrix SE(3). + Returns the corresponding 6x6 adjoint representation [AdT] + +Example: + Input: + AdT = Adjoint[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}] + Output: + {{1,0,0,0,0,0},{0,0,-1,0,0,0},{0,1,0,0,0,0},{0,0,3,1,0,0}, + {3,0,0,0,0,-1},{0,0,0,0,1,0}}" + + +ScrewToAxis::usage= +"ScrewToAxis[q,s,h]: +Takes + q: a point lying on the screw axis, + s: a unit vector in the direction of the screw axis, + h: the pitch of the screw axis. +Returns the corresponding normalized screw axis. + +Example: + Input: + S = ScrewToAxis[{{3,0,0}}\[Transpose],{{0,0,1}}\[Transpose],2] + Output: + {{0},{0},{1},{0},{-3},{2}}" + + +AxisAng6::usage= +"AxisAng6[expc6]: + Takes a 6-vector of exponential coordinates for rigid-body motion + S*theta. + Returns + S: the corresponding normalized screw axis, + theta: the distance traveled along/about S. + The first element of the output is the screw axis S and the second is the + distance theta. + +Example: + Input: + {S, theta} = AxisAng6[{{1},{0},{0},{1},{2},{3}}] + Output: + {{{1},{0},{0},{1},{2},{3}},1}" + + +MatrixExp6::usage= +"MatrixExp6[se3mat]: +Takes a se(3) representation of exponential coordinates. +Returns a T matrix SE(3) that is achieved by traveling along/about the screw +axis S for a distance theta from an initial configuration T=I. + +Example: + Input: + T = MatrixExp6[ + {{0,0,0,0},{0,0,-1.5707963267948966,2.3561944901923448}, + {0,1.5707963267948966,0,2.3561944901923448},{0,0,0,0}} + ] + Output: + {{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}" + + +MatrixLog6::usage= +"MatrixLog6[T]: +Takes a transformation matrix T in SE(3). +Returns the corresponding se(3) representation of exponential coordinates. + +Example: + Input: + se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]//N + Output: + {{0,0,0,0},{0,0,-1.5708,2.35619},{0,1.5708,0,2.35619},{0,0,0,0}}" + + +DistanceToSE3::usage= +"DistanceToSE3[mat]: +Takes mat as a 4x4 matrix. +Returns the Frobenius norm to describe the distance of mat from the SE(3) +manifold. +Computes the determinant of matR, the top 3x3 submatrix of mat. If +det (matR) <= 0, return a large number. If det (mat) > 0, replace the top 3x3 +submatrix of mat with matR'.matR, and set the first three entries of the +fourth column of mat to zero. Then return norm (mat - I). + +Example : + Input: + d = DistanceToSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5}, + {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}] + Output: + 0.134931" + + +TestIfSE3::usage= +"TestIfSE3[mat]: +Takes mat as a 4X4 matrix. +Check if mat is close to or on the manifold SE(3). + +Example: + Input: + judge = TestIfSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5}, + {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}] + Output: + False" + + +ProjectToSE3::usage= +"ProjectToSE3[mat]: +Takes a matrix near SE(3) to project to SE(3). +Returns the closest rotation matrix that is in SE(3). +This function uses singular-value decomposition (see +http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review) +and is only appropriate for matrices close to SE(3). + +Example: + Input: + T = ProjectToSE3[{{0.675,0.150,0.720,1.2}, + {0.370,0.771,-0.511,5.4}, + {-0.630,0.619,0.472,3.6}, + {0.003,0.002,0.010,0.9}}] + Output: + {{0.679011,0.148945,0.718859,1.2},{0.373207,0.773196,-0.512723,5.4}, + {-0.632187,0.616428,0.469421,3.6},{0,0,0,1}}" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 4: FORWARD KINEMATICS*) + + +FKinBody::usage= +"FKinBody[M,Blist,thetalist]: +Takes + M: The home configuration (position and orientation) of the end-effector, + Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a matrix + with axes as the columns, + thetalist: A list of joint coordinates. +Returns T in SE(3) representing the end-effector frame when the joints are at +the specified coordinates (i.t.o Body Frame). + +Example: + Inputs: + M = {{-1,0,0,0},{0,1,0,6},{0,0,-1,2},{0,0,0,1}}; + Blist = {{0,0,-1,2,0,0},{0,0,0,0,1,0},{0,0,1,0,0,0.1}}\[Transpose]; + thetalist = {{(Pi/2.0),3,Pi}}\[Transpose]; + T = FKinBody[M,Blist,thetalist] + Output: + {{0,1,0,-5},{1,0,0,4},{0,0,-1,1.68584},{0,0,0,1}}" + + +FKinSpace::usage= +"FKinSpace[M,Slist,thetalist]: +Takes + M: the home configuration (position and orientation) of the end-effector, + Slist: The joint screw axes in the space frame when the manipulator is at + the home position, in the format of a matrix with screw axes as + the columns, + thetalist: A list of joint coordinates. +Returns T in SE(3) representing the end-effector frame when the joints are at +the specified coordinates (i.t.o Fixed Space Frame). + +Example: + Inputs: + M = {{-1,0,0,0},{0,1,0,6},{0,0,-1,2},{0,0,0,1}}; + Slist = {{0,0,1,4,0,0},{0,0,0,0,1,0},{0,0,-1,-6,0,-0.1}}\[Transpose]; + thetalist = {{Pi/2,3,Pi}}\[Transpose]; + T = FKinSpace[M,Slist,thetalist] + Output: + {{0,1,0,-5},{1,0,0,4},{0,0,-1,1.68584},{0,0,0,1}}" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 5: VELOCITY KINEMATICS AND STATICS*) + + +JacobianBody::usage= +"JacobianBody[Blist,thetalist]: +Takes + Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a matrix + with axes as the columns, + thetalist:A list of joint coordinates. +Returns the corresponding body Jacobian (6xn real numbers). + +Example: + Inputs: + Blist = {{0,0,1,0,0.2,0.2},{1,0,0,2,0,3},{0,1,0,0,2,1}, + {1,0,0,0.2,0.3,0.4}}\[Transpose]; + thetalist = {{0.2,1.1,0.1,1.2}}\[Transpose]; + Jb = JacobianBody[Blist,thetalist] + Output: + {{-0.0452841,0.995004,0,1},{0.743593,0.0930486,0.362358,0}, + {-0.667097,0.0361754,-0.932039,0},{2.32586,1.66809,0.564108,0.2}, + {-1.44321,2.94561,1.43307,0.3},{-2.0664,1.82882,-1.58869,0.4}}" + + +JacobianSpace::usage= +"JacobianSpace[Slist,thetalist]: +Takes + Slist: The joint screw axes in the space frame when the manipulator is at + the home position, in the format of a matrix with axes as the + columns, + thetalist: A list of joint coordinates. +Returns the corresponding space Jacobian (6xn real numbers). + +Example: + Inputs: + Slist = {{0,0,1,0,0.2,0.2},{1,0,0,2,0,3},{0,1,0,0,2,1}, + {1,0,0,0.2,0.3,0.4}}\[Transpose]; + thetalist = {{0.2,1.1,0.1,1.2}}\[Transpose]; + Js = JacobianSpace[Slist, thetalist] + Ouput: + {{0,0.980067,-0.0901156,0.957494},{0,0.198669,0.444554,0.284876}, + {1,0,0.891207,-0.0452841},{0,1.95219,-2.21635,-0.511615}, + {0.2,0.436541,-2.43713,2.77536},{0.2,2.96027,3.23573,2.22512}}" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 6: INVERSE KINEMATICS*) + + +IKinBody::usage= +"IKinBody[Blist,M,T,thetalist0,eomg,ev]: +Takes + Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a matrix + with axes as the columns, + M: The home configuration of the end-effector, + T: The desired end-effector configuration T, + thetalist0: An initial guess of joint angles that are close to satisfying + T, + eomg: A small positive tolerance on the end-effector orientation error. + The returned joint angles must give an end-effector orientation + error less than eomg, + ev: A small positive tolerance on the end-effector linear position error. + The returned joint angles must give an end-effector position error + less than ev. +Returns + thetalist: Joint angles that achieve T within the specified tolerance, + success: A logical value where True means that the function found a + solution and False means that it ran through the set number of + maximum iterations without finding a solution within the + tolerances eomg and ev. +Uses an iterative Newton-Raphson root-finding method. +The maximum number of iterations before the algorithm is terminated has been +hardcoded in as a variable called maxiterations.It is set to 20 at the start +of the function,but can be changed if needed. + +Example: + Inputs: + Blist = {{0,0,-1,2,0,0},{0,0,0,0,1,0},{0,0,1,0,0,0.1}}\[Transpose]; + M = {{-1,0,0,0},{0,1,0,6},{0,0,-1,2},{0,0,0,1}}; + T = {{0,1,0,-5},{1,0,0,4},{0,0,-1,1.6858},{0,0,0,1}}; + thetalist0 = {{1.5,2.5,3}}\[Transpose]; + eomg = 0.01; + ev = 0.001; + {thetalist,success} = IKinBody[Blist,M,T,thetalist0,eomg,ev] + Ouput: + {{{1.57074},{2.99967},{3.14154}},True}" + + +IKinSpace::usage= +"IKinSpace[Slist,M,T,thetalist0,eomg,ev]: +Takes + Slist: The joint screw axes in the space frame when the manipulator is at + the home position, in the format of a matrix with axes as the + columns, + M: The home configuration of the end-effector, + T: The desired end-effector configuration T, + thetalist0: An initial guess of joint angles that are close to satisfying + T, + eomg: A small positive tolerance on the end-effector orientation error. + The returned joint angles must give an end-effector orientation + error less than eomg, + ev: A small positive tolerance on the end-effector linear position error. + The returned joint angles must give an end-effector position error + less than ev. +Returns + thetalist: Joint angles that achieve T within the specified tolerances, + success: A logical value where True means that the function found a + solution and False means that it ran through the set number of + maximum iterations without finding a solution within the + tolerances eomg and ev. +Uses an iterative Newton-Raphson root-finding method. +The maximum number of iterations before the algorithm is terminated has been +hardcoded in as a variable called maxiterations.It is set to 20 at the start +of the function,but can be changed if needed. + +Exapmle: + Inputs: + Slist = {{0,0,1,4,0,0},{0,0,0,0,1,0},{0,0,-1,-6,0,-0.1}}\[Transpose]; + M = {{-1,0,0,0},{0,1,0,6},{0,0,-1,2},{0,0,0,1}}; + T = {{0,1,0,-5},{1,0,0,4},{0,0,-1,1.6858},{0,0,0,1}}; + thetalist0 = {{1.5,2.5,3}}\[Transpose]; + eomg = 0.01; + ev = 0.001; + {thetalist,success} = IKinSpace[Slist,M,T,thetalist0,eomg,ev] + Ouput: + {{{1.57074},{2.99966},{3.14153}},True}" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 8: DYNAMICS OF OPEN CHAINS*) + + +ad::usage= +"ad[V]: + Takes a 6-vector spatial velocity. + Returns the corresponding 6x6 matrix [adV]. + Used to calculate the Lie bracket [V1, V2] = [adV1] V2 + +Example: + Input: + mat = ad[{{1,2,3,4,5,6}}\[Transpose]] + Output: + {{0,-3,2,0,0,0},{3,0,-1,0,0,0},{-2,1,0,0,0,0},{0,-6,5,0,-3,2}, + {6,0,-4,3,0,-1},{-5,4,0,-2,1,0}}" + + +InverseDynamics::usage= +"InverseDynamics[thetalist,dthetalist,ddthetalist,g,Ftip,Mlist,Glist,Slist]: +Takes + thetalist: n-vector of joint variables, + dthetalist: n-vector of joint rates, + ddthetalist: n-vector of joint accelerations, + g: Gravity vector g, + Ftip: Spatial force applied by the end-effector expressed in frame {n+1}, + Mlist:L ist of link frames {i} relative to {i-1} at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns taulist:The n-vector of required joint forces/torques. +This function uses forward-backward Newton-Euler iterations to solve the +equation: +taulist=Mlist(thetalist)ddthetalist+c(thetalist,dthetalist)+g(thetalist) + +Jtr(thetalist)Ftip + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + ddthetalist = {{2,1.5,1}}\[Transpose]; + g = {{0,0,-9.8}}\[Transpose]; + Ftip = {{1,1,1,1,1,1}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + taulist = InverseDynamics[thetalist,dthetalist,ddthetalist,g,Ftip, + Mlist,Glist,Slist] + Output: + {{74.6962},{-33.0677},{-3.23057}}" + + +MassMatrix::usage= +"MassMatrix[thetalist,Mlist,Glist,Slist]: +Takes + thetalist: A list of joint variables,Mlist:List of link frames i relative + to i-1 at the home position,Glist:Spatial inertia matrices Gi + of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns M: The numerical inertia matrix M(thetalist) of an n-joint serial + chain at the given configuration thetalist. +This function calls InverseDynamics n times, each time passing a ddthetalist +vector with a single element equal to one and all other inputs set to zero. +Each call of InverseDynamics generates a single column,and these columns are +assembled to create the inertia matrix. + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + M = MassMatrix[thetalist,Mlist,Glist,Slist] + Output: + {{22.5433,-0.307147,-0.00718426},{-0.307147,1.96851,0.432157}, + {-0.00718426,0.432157,0.191631}}" + + +VelQuadraticForces::usage= +"VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist]: +Takes + thetalist: A list of joint variables, + dthetalist: A list of joint rates, + Mlist: List of link frames i relative to i-1 at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns c: The vector c(thetalist,dthetalist) of Coriolis and centripetal + terms for a given thetalist and dthetalist. +This function calls InverseDynamics with g=0,Ftip=0,and ddthetalist=0. + +Example(3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + c = VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist] + Output: + {{0.264531},{-0.0550516},{-0.00689132}}" + + +GravityForces::usage= +"GravityForces[thetalist,g,Mlist,Glist,Slist]: +Takes + thetalist: A list of joint variables, + g: 3-vector for gravitational acceleration, + Mlist: List of link frames i relative to i-1 at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns grav: The joint forces/torques required to overcome gravity at +thetalist. +This function calls InverseDynamics with Ftip=0,dthetalist=0, and +ddthetalist=0. + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + g = {{0,0,-9.8}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + grav = GravityForces[thetalist,g,Mlist,Glist,Slist] + Output: + {{28.4033},{-37.6409},{-5.44159}}" + + +EndEffectorForces::usage= +"EndEffectorForces[thetalist,Ftip,Mlist,Glist,Slist]: +Takes + thetalist: A list of joint variables, + Ftip: Spatial force applied by the end-effector expressed in frame {n+1}, + Mlist: List of link frames i relative to i-1 at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns JTFtip: The joint forces and torques required only to create the end- + effector force Ftip. +This function calls InverseDynamics with g=0,dthetalist=0,and ddthetalist=0. + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + Ftip = {{1,1,1,1,1,1}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + JTFtip = EndEffectorForces[thetalist,Ftip,Mlist,Glist,Slist] + Output: + {{1.40955}, {1.85771}, {1.39241}}" + + +ForwardDynamics::usage= +"ForwardDynamics[thetalist,dthetalist,taulist,g,Ftip,Mlist,Glist,Slist]: +Takes + thetalist: A list of joint variables, + dthetalist: A list of joint rates, + taulist: An n-vector of joint forces/torques, + g: Gravity vector g, + Ftip: Spatial force applied by the end-effector expressed in frame {n+1}, + Mlist: List of link frames i relative to i-1 at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns ddthetalist: The resulting joint accelerations. +This function computes ddthetalist by solving: +Mlist(thetalist).ddthetalist +=taulist-c(thetalist,dthetalist)-g(thetalist)-Jtr(thetalist)Ftip + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + taulist = {{0.5,0.6,0.7}}\[Transpose]; + g = {{0,0,-9.8}}\[Transpose]; + Ftip = {{1,1,1,1,1,1}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + ddthetalist = ForwardDynamics[thetalist,dthetalist,taulist,g,Ftip, + Mlist,Glist,Slist] + Output: + {{-0.973929}, {25.5847}, {-32.915}}" + + +EulerStep::usage= +"EulerStep[thetalist,dthetalist,ddthetalist,dt]: +Takes + thetalist: n-vector of joint variables, + dthetalist: n-vector of joint rates, + ddthetalist: n-vector of joint accelerations, + dt: The timestep delta t. +Returns + thetalistNext: Vector of joint variables after dt from first order Euler + integration, + dthetalistNext: Vector of joint rates after dt from first order Euler + integration. + +Example (3 Link Robot): + Inputs\:ff1a + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + ddthetalist = {{2,1.5,1}}\[Transpose]; + dt = 0.1; + {thetalistNext,dthetalistNext} = EulerStep[thetalist,dthetalist, + ddthetalist,dt] + Output: + {{{0.11},{0.12},{0.13}},{{0.3},{0.35},{0.4}}}" + + +InverseDynamicsTrajectory::usage= +"InverseDynamicsTrajectory[thetamat,dthetamat,ddthetamat,g,Ftipmat,Mlist, + Glist,Slist]: +Takes + thetamat: An N x n matrix of robot joint variables, + dthetamat: An N x n matrix of robot joint velocities, + ddthetamat: An N x n matrix of robot joint accelerations, + g: Gravity vector g, + Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector + (If there are no tip forces the user should input a zero and a + zero matrix will be used), + Mlist: List of link frames i relative to i-1 at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns. +Returns taumat: The N x n matrix of joint forces/torques for the specified + trajectory,where each of the N rows is the vector of joint + forces/torques at each time step. +This function uses InverseDynamics to calculate the joint forces/torques +required to move the serial chain along the given trajectory. + +Example (3 Link Robot): + Inputs: + (*Create a trajectory to follow using functions from Chapter*) + thetastart = {{0,0,0}}\[Transpose]; + thetaend = {{Pi/2,Pi/2,Pi/2}}\[Transpose]; + Tf = 3; + No = 1000; + method = 5; + traj = JointTrajectory[thetastart,thetaend,Tf,No,method]; + thetamat = traj; + dthetamat = traj; + ddthetamat = traj; + dt = Tf/(No-1.0); + Do[ + dthetamat[[i,;;]] = (thetamat[[i,;;]]-thetamat[[i-1,;;]])/dt; + ddthetamat[[i,;;]] = (dthetamat[[i,;;]]-dthetamat[[i-1,;;]])/dt, + {i,2,Dimensions[traj][[1]]} + ]; + (*Initialise robot descripstion*) + g = {{0,0,-9.8}}\[Transpose]; + Ftipmat = ConstantArray[1,{No,6}]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + taumat = InverseDynamicsTrajectory[thetamat,dthetamat,ddthetamat,g, + Ftipmat,Mlist,Glist,Slist]; + (*Output to plot the joint forces/torques*) + Tau1 = Flatten[taumat[[;;,1]]]; + Tau2 = Flatten[taumat[[;;,2]]]; + Tau3 =Flatten[ taumat[[;;,3]]]; + (*PLEASE REPLACE THE SINGLE QUOTES TO DOUBLE TUOTES + BEFORE RUNNING THE COMMAND BELOW*) + ListLinePlot[ + {Tau1,Tau2,Tau3}, + PlotLegends\[Rule]{'Tau1','Tau2','Tau3'}, + AxesLabel\[Rule]{'Time','Torque'}, + PlotLabel\[Rule]'Plot of Torque Trajectories', + PlotRange\[Rule]Full + ]" + + +ForwardDynamicsTrajectory::usage= +"ForwardDynamicsTrajectory[thetalist,dthetalist,taumat,g,Ftipmat,Mlist,Glist, + Slist,dt,intRes]: +Takes + thetalist: n-vector of initial joint variables, + dthetalist: n-vector of initial joint rates, + taumat: An N x n matrix of joint forces/torques,where each row is the joint + effort at any time step, + g:Gravity vector g, + Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector + (If there are no tip forces the user should input a zero and a + zero matrix will be used), + Mlist: List of link frames {i} relative to {i-1} at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns, + dt: The timestep between consecutive joint forces/torques, + intRes: Integration resolution is the number of times integration (Euler) + takes places between each time step. Must be an integer value + greater than or equal to 1. +Returns thetamat: The N x n matrix of robot joint angles resulting from the +specified joint forces/torques,dthetamat:The N x n matrix of robot joint +velocities. +This function simulates the motion of a serial chain given an open-loop +history of joint forces/torques. It calls a numerical integration procedure +that uses ForwardDynamics. + +Example: + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + taumat = {{3.63,-6.58,-5.57},{3.74,-5.55,-5.5},{4.31,-0.68,-5.19}, + {5.18,5.63,-4.31},{5.85,8.17,-2.59},{5.78,2.79,-1.7}, + {4.99,-5.3,-1.19},{4.08,-9.41,0.07},{3.56,-10.1,0.97}, + {3.49,-9.41,1.23}}; + (*Initialise robot descripstion (Example with 3 links)*) + g = {{0,0,-9.8}}\[Transpose]; + Ftipmat = ConstantArray[1,{Dimensions[taumat][[1]],6}]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + dt = 0.1; + intRes = 8; + {thetamat,dthetamat} = ForwardDynamicsTrajectory[ + thetalist,dthetalist,taumat,g,Ftipmat,Mlist,Glist,Slist,dt,intRes + ]; + (*Output to plot the joint forces/torques*) + theta1 = Flatten[thetamat[[;;,1]]]; + theta2 = Flatten[thetamat[[;;,2]]]; + theta3 = Flatten[thetamat[[;;,3]]]; + dtheta1 = Flatten[ dthetamat[[;;,1]]]; + dtheta2 = Flatten[dthetamat[[;;,2]]]; + dtheta3 = Flatten[ dthetamat[[;;,3]]]; + (*PLEASE REPLACE THE SINGLE QUOTES TO DOUBLE TUOTES + BEFORE RUNNING THE COMMAND BELOW*) + ListLinePlot[ + {theta1,theta2,theta3,dtheta1,dtheta2,dtheta3}, + PlotLegends->{'theta1','theta2','theta3', + 'dtheta1','dtheta2','dtheta3'}, + AxesLabel->{'Time','Joint Angles/Velocities'}, + PlotLabel->'Plot of Joint Angles and Joint Velocities', + PlotRange->Full + ]" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 9: TRAJECTORY GENERATION*) + + +CubicTimeScaling::usage= +"CubicTimeScaling[Tf,t]: +Takes + Tf: Total time of the motion in seconds from rest to rest, + t: The current time t satisfying 01 (Start and stop) in the discrete + representation of the trajectory, + method: The time-scaling method,where 3 indicates cubic (third-order + polynomial) time scaling and 5 indicates quintic (fifth-order + polynomial) time scaling. +Returns traj: A trajectory as an N x n matrix, where each row is an n-vector + of joint variables at an instant in time. The first row is + thetastart and the Nth row is thetaend.The elapsed time between + each row is Tf/(N-1). +The returned trajectory is a straight-line motion in joint space. + +Example: + Inputs: + thetastart = {{1,0,0,1,1,0.2,0,1}}\[Transpose]; + thetaend = {{1.2,0.5,0.6,1.1,2,2,0.9,1}}\[Transpose]; + Tf = 4; + Np = 6; + method = 3; + traj = JointTrajectory[thetastart,thetaend,Tf,Np,method]//N + Output: + {{1,0,0,1,1,0.2,0,1}, + {1.0208,0.052,0.0624,1.0104,1.104,0.3872,0.0936,1}, + {1.0704,0.176,0.2112,1.0352,1.352,0.8336,0.3168,1}, + {1.1296,0.324,0.3888,1.0648,1.648,1.3664,0.5832,1}, + {1.1792,0.448,0.5376,1.0896,1.896,1.8128,0.8064,1}, + {1.2,0.5,0.6,1.1,2,2,0.9,1}}" + + +ScrewTrajectory::usage= +"ScrewTrajectory[Xstart,Xend,Tf,N,method]: +Takes + Xstart: The initial end-effector configuration, + Xend:The final end-effector configuration, + Tf:Total time of the motion in seconds from rest to rest, + N: The number of points N>1 (Start and stop) in the discrete + representation of the trajectory, + method: The time-scaling method, where 3 indicates cubic (third-order + polynomial) time scaling and 5 indicates quintic (fifth-order + polynomial) time scaling. +Returns traj: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1).The first in the list is Xstart + and the Nth is Xend. +This function calculates a trajectory corresponding to the screw motion about +a space screw axis. + +Example: + Inputs: + Xstart={{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}}; + Xend={{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}; + Tf=5; + Np=4; + method=3; + traj = ScrewTrajectory[Xstart,Xend,Tf,Np,method] + Output: + {{{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}}, + {{0.904111,-0.250372,0.346261,0.440958}, + {0.346261,0.904111,-0.250372,0.528746}, + {-0.250372,0.346261,0.904111,1.60067}, + {0,0,0,1}}, + {{0.346261,-0.250372,0.904111,-0.117111}, + {0.904111,0.346261,-0.250372,0.472742}, + {-0.250372,0.904111,0.346261,3.274}, + {0,0,0,1}}, + {{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}}" + + +CartesianTrajectory::usage= +"CartesianTrajectory[Xstart,Xend,Tf,N,method]: + Takes + Xstart: The initial end-effector configuration, + Xend: The final end-effector configuration, + Tf: Total time of the motion in seconds from rest to rest, + N: The number of points N>1 (Start and stop) in the discrete + representation of the trajectory, + method: The time-scaling method,where 3 indicates cubic (third-order + polynomial) time scaling and 5 indicates quintic (fifth-order + polynomial) time scaling. + Returns traj: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1).The first in the list is + Xstart and the Nth is Xend. + This function is similar to ScrewTrajectory,except the origin of the end- + effector frame follows a straight line,decoupled from the rotational + motion. + +Example: + Inputs: + Xstart = {{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}}; + Xend = {{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}; + Tf = 5; + Np = 4; + method = 5; + traj = CartesianTrajectory[Xstart,Xend,Tf,Np,method]//N; + Output: + {{{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}}, + {{0.936625,-0.214001,0.277376,0.811111}, + {0.277376,0.936625,-0.214001,0}, + {-0.214001,0.277376,0.936625,1.65062},{0,0,0,1}}, + {{0.277376,-0.214001,0.936625,0.288889}, + {0.936625,0.277376,-0.214001,0}, + {-0.214001,0.936625,0.277376,3.44938},{0,0,0,1}}, + {{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}}" + + +(* ::Subsection::Closed:: *) +(*CHAPTER 11: ROBOT CONTROL*) + + +ComputedTorque::usage= +"ComputedTorque[thetalist,dthetalist,eint,g,Mlist,Glist,Slist,thetalistd, +dthetalistd,ddthetalistd,Kp,Ki,Kd]: +Takes + thetalist: n-vector of joint variables, + dthetalist: n-vector of joint rates, + eint: n-vector of the time-integral of joint errors, + g: Gravity vector g, + Mlist: List of link frames {i} relative to {i-1} at the home position, + Glist: Spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns, + thetalistd: n-vector of reference joint variables, + dthetalistd: n-vector of reference joint velocities, + ddthetalistd: n-vector of reference joint accelerations, + Kp: The feedback proportional gain (identical for each joint), + Ki: The feedback integral gain (identical for each joint), + Kd: The feedback derivative gain (identical for each joint). +Returns taulist: The vector of joint forces/torques computed by the feedback + linearizing controller at the current instant. + +Example: + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + eint = {{0.2,0.2,0.2}}\[Transpose]; + g = {{0,0,-9.8}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + thetalistd = {{1,1,1}}\[Transpose]; + dthetalistd = {{2,1.2,2}}\[Transpose]; + ddthetalistd = {{0.1,0.1,0.1}}\[Transpose]; + Kp = 1.3; + Ki = 1.2; + Kd = 1.1; + taulist = ComputedTorque[thetalist,dthetalist,eint,g,Mlist,Glist, + Slist,thetalistd,dthetalistd,ddthetalistd, + Kp,Ki,Kd] + Ouput: + {{133.005}, {-29.9422}, {-3.03277}}" + + +SimulateControl::usage= +"SimulateControl[thetalist,dthetalist,g,Ftipmat,Mlist,Glist,Slist,thetamatd, + dthetamatd,ddthetamatd,gtilde,Mtildelist,Gtildelist,Kp,Ki, + Kd,dt,intRes]: +Takes + thetalist: n-vector of initial joint variables, + dthetalist: n-vector of initial joint velocities, + g: Actual gravity vector g, + Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector + (If there are no tip forces the user should input a zero and a + zero matrix will be used), + Mlist: Actual list of link frames i relative to i-1 at the home position, + Glist: Actual spatial inertia matrices Gi of the links, + Slist: Screw axes Si of the joints in a space frame, in the format of a + matrix with axes as the columns, + thetamatd: An Nxn matrix of desired joint variables from the reference + trajectory, + dthetamatd: An Nxn matrix of desired joint velocities, + ddthetamatd: An Nxn matrix of desired joint accelerations, + gtilde: The gravity vector based on the model of the actual robot (actual + values given above), + Mtildelist: The link frame locations based on the model of the actual + robot (actual values given above), + Gtildelist: The link spatial inertias based on the model of the actual + robot (actual values given above), + Kp: The feedback proportional gain (identical for each joint), + Ki: The feedback integral gain (identical for each joint), + Kd: The feedback derivative gain (identical for each joint), + dt:The timestep between points on the reference trajectory, + intRes: Integration resolution is the number of times integration (Euler) + takes places between each time step.Must be an integer value + greater than or equal to 1. +Returns + taumat: An Nxn matrix of the controllers commanded joint forces/torques, + where each row of n forces/torques corresponds to a single time + instant, + thetamat: An Nxn matrix of actual joint angles. +The end of this function plots all the actual and desired joint angles. + +Example (3 Link Robot): + Inputs: + thetalist = {{0.1,0.1,0.1}}\[Transpose]; + dthetalist = {{0.1,0.2,0.3}}\[Transpose]; + (*Initialize robot description*) + g = {{0,0,-9.8}}\[Transpose]; + M01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.089159},{0,0,0,1}}; + M12 = {{0,0,1,0.28},{0,1,0,0.13585},{-1,0,0,0},{0,0,0,1}}; + M23 = {{1,0,0,0},{0,1,0,-0.1197},{0,0,1,0.395},{0,0,0,1}}; + M34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.14225},{0,0,0,1}}; + G1 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}]; + G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}]; + G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095, + 2.275,2.275,2.275}]; + Glist = {G1,G2,G3}; + Mlist = {M01,M12,M23,M34}; + Slist = {{1,0,1,0,1,0},{0,1,0,-0.089,0,0}, + {0,1,0,-0.089,0,0.425}}\[Transpose]; + dt = 0.01; + (*Create a trajectory to follow*) + thetaend = {{Pi/2,Pi,1.5*Pi}}\[Transpose]; + Tf = 1; + No = Tf/dt; + method = 5; + traj = JointTrajectory[thetalist,thetaend,Tf,No,method]; + thetamatd = traj; + dthetamatd = traj; + ddthetamatd = traj; + dt = Tf/(No-1.0); + Do[ + dthetamatd[[i,;;]]=(thetamatd[[i,;;]]-thetamatd[[i-1,;;]])/dt; + ddthetamatd[[i,;;]]=(dthetamatd[[i,;;]]-dthetamatd[[i-1,;;]])/dt, + {i,2,Dimensions[traj][[1]]} + ]; + (*Possibly wrong robot description*) + gtilde = {{0.8,0.2,-8.8}}\[Transpose]; + Mhat01 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.1},{0,0,0,1}}; + Mhat12 = {{0,0,1,0.3},{0,1,0,0.2},{-1,0,0,0},{0,0,0,1}}; + Mhat23 = {{1,0,0,0},{0,1,0,-0.2},{0,0,1,0.4},{0,0,0,1}}; + Mhat34 = {{1,0,0,0},{0,1,0,0},{0,0,1,0.2},{0,0,0,1}}; + Ghat1 = DiagonalMatrix[{0.1,0.1,0.1,4,4,4}]; + Ghat2 = DiagonalMatrix[{0.3,0.3,0.1,9,9,9}]; + Ghat3 = DiagonalMatrix[{0.1,0.1,0.1,3,3,3}]; + Gtildelist = {Ghat1,Ghat2,Ghat3}; + Mtildelist = {Mhat01,Mhat12,Mhat23,Mhat34}; + Ftipmat = ConstantArray[1,{Dimensions[traj][[1]],6}]; + Kp = 20; + Ki = 10; + Kd = 18; + intRes = 8; + {taumat,thetamat} = + SimulateControl[thetalist,dthetalist,g,Ftipmat,Mlist,Glist,Slist, + thetamatd,dthetamatd,ddthetamatd,gtilde, + Mtildelist,Gtildelist,Kp,Ki,Kd,dt,intRes];" + + +(* ::Section::Closed:: *) +(*CODE*) + + +Begin["`Private`"]; + + +(* ::Subsection::Closed:: *) +(*BASIC HELPER FUNCTIONS*) + + +NearZero[s_]:=Abs[s]<10^-6 + + +(* ::Subsection::Closed:: *) +(*CHAPTER 3: RIGID-BODY MOTION*) + + +RotInv[R_]:=R\[Transpose] + + +VecToso3[omg_]:={{0,-omg[[3,1]],omg[[2,1]]},{omg[[3,1]],0,-omg[[1,1]]}, + {-omg[[2,1]],omg[[1,1]],0}} + + +so3ToVec[so3mat_]:={{so3mat[[3,2]],so3mat[[1,3]],so3mat[[2,1]]}}\[Transpose] + + +AxisAng3[expc3_]:={expc3/Norm[expc3],Norm[expc3]} + + +MatrixExp3[so3mat_]:=Module[ + {omgtheta,omghat,theta,omgmat}, + omgtheta=so3ToVec[so3mat]; + If[ + NearZero[Norm[omgtheta]], + Return[IdentityMatrix[3]], + {omghat, theta}=AxisAng3[omgtheta]; + omgmat=so3mat/theta; + Return[IdentityMatrix[3]+Sin[theta]*omgmat+ + (1-Cos[theta])*omgmat.omgmat] + ] +] + + +MatrixLog3[R_]:=Module[ + {acosinput,theta,omgmat}, + acosinput=(Tr[R]-1)/2; + Which[ + acosinput>=1, + Return[ConstantArray[0,{3,3}]], + acosinput<=-1, + Which[ + Not[NearZero[R[[3,3]]+1]], + Return[VecToso3[ + 1/Sqrt[2(1+R[[3,3]])]* + {{R[[1,3]],R[[2,3]],1+R[[3,3]]}}\[Transpose]*Pi + ]], + Not[NearZero[R[[2,2]]+1]], + Return[VecToso3[ + 1/Sqrt[2(1+R[[2,2]])]* + {{R[[1,2]],1+R[[2,2]],R[[3,2]]}}\[Transpose]*Pi + ]], + True, + Return[VecToso3[ + 1/Sqrt[2(1+R[[1,1]])]* + {{1+R[[1,1]],R[[2,1]],R[[3,1]]}}\[Transpose]*Pi + ]] + ], + True, + theta=ArcCos[acosinput]; + omgmat=(R-R\[Transpose])/(2*Sin[theta]); + Return[theta*omgmat] + ] +] + + +DistanceToSO3[mat_]:=If[ + Det[mat]>0, + Return[Norm[mat\[Transpose].mat-IdentityMatrix[3],"Frobenius"]], + Return[10^9] +] + + +TestIfSO3[mat_]:=Abs[DistanceToSO3[mat]]<10^-3 + + +ProjectToSO3[mat_]:=Module[ + {R,u,w,v}, + {u,w,v}=SingularValueDecomposition[mat]; + R=u.v\[Transpose]; + (*In the case below, the result may be far from mat.*) + If[ + Det[R] < 0, + Return[ArrayFlatten[{{R[[;;,1;;2]],{-R[[;;,3]]}\[Transpose]}}]], + Return[R] + ] +] + + +RpToTrans[R_,p_]:=ArrayFlatten[{{R,p},{0,1}}] + + +TransToRp[T_]:={T[[1;;3,1;;3]],{T[[1;;3,4]]}\[Transpose]} + + +TransInv[T_]:=ArrayFlatten[ + {{T[[1;;3,1;;3]]\[Transpose], + -T[[1;;3,1;;3]]\[Transpose].{T[[1;;3,4]]}\[Transpose]}, + {0,1}} +] + + +VecTose3[V_]:=ArrayFlatten[ + {{VecToso3[{V[[1;;3,1]]}\[Transpose]],{V[[4;;6,1]]}\[Transpose]},{0,0}} +] + + +se3ToVec[se3mat_]:={{se3mat[[3,2]],se3mat[[1,3]],se3mat[[2,1]], + se3mat[[1,4]],se3mat[[2,4]],se3mat[[3,4]]}}\[Transpose] + + +Adjoint[T_]:=Module[ + {R,p}, + {R,p}=TransToRp[T]; + Return[ArrayFlatten[{{R,0},{VecToso3[p].R,R}}]] +] + + +ScrewToAxis[q_,s_,h_]:=ArrayFlatten[{{s},{{Cross[Flatten[q], + Flatten[s]]}\[Transpose]+h*s}}] + + +AxisAng6[expc6_]:=Module[ + {theta}, + theta=Norm[{expc6[[1;;3,1]]}\[Transpose]]; + If[NearZero[theta],theta=Norm[{expc6[[4;;6,1]]}\[Transpose]]]; + Return[{expc6/theta,theta}] +] + + +MatrixExp6[se3mat_]:=Module[ + {omgtheta,omghat,theta,omgmat,G}, + omgtheta=so3ToVec[se3mat[[1;;3,1;;3]]]; + If[ + NearZero[Norm[omgtheta]], + Return[ArrayFlatten[ + {{IdentityMatrix[3],{se3mat[[1;;3,4]]}\[Transpose]},{0,1}} + ]], + {omghat,theta} = AxisAng3[omgtheta]; + omgmat=se3mat[[1;;3,1;;3]]/theta; + Return[ArrayFlatten[ + {{MatrixExp3[se3mat[[1;;3,1;;3]]], + (IdentityMatrix[3]*theta+(1-Cos[theta])*omgmat+ + (theta-Sin[theta])*omgmat.omgmat). + ({se3mat[[1;;3,4]]}\[Transpose]/theta)}, + {0,1}} + ]] + ] +] + + +MatrixLog6[T_]:=Module[ + {R,p,acosinput,theta,omgmat}, + {R,p}=TransToRp[T]; + omgmat=MatrixLog3[R]; + If[ + omgmat==ConstantArray[0,{3,3}], + ArrayFlatten[ + {{ConstantArray[0,{3,3}],{T[[1;;3,4]]}\[Transpose]},{0,0}} + ], + theta=ArcCos[(Tr[R]-1)/2]; + Return[ArrayFlatten[{ + {omgmat,(IdentityMatrix[3]-omgmat/2+ + (1/theta-Cot[theta/2]/2)*omgmat.omgmat/theta).p}, + {0,0} + }]] + ] +] + + +DistanceToSE3[mat_]:=Module[ + {R,p}, + {R,p}=TransToRp[mat]; + If[ + Det[R] > 0, + Return[Norm[ + Join[Join[R\[Transpose].R,{{0,0,0}}\[Transpose],2],{mat[[4,;;]]}]- + IdentityMatrix[4], + "Frobenius" + ]], + Return[10^9] + ] +] + + +TestIfSE3[mat_]:=Abs[DistanceToSE3[mat]]<10^-3 + + +ProjectToSE3[mat_]:=RpToTrans[ProjectToSO3[mat[[1;;3,1;;3]]], + {mat[[1;;3,4]]}\[Transpose]] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 4: FORWARD KINEMATICS*) + + +FKinBody[M_,Blist_,thetalist_]:=Module[ + {n,T,i}, + T=M; + Do[ + T=T.MatrixExp6[ + VecTose3[{Blist[[;;,i]]}\[Transpose]*thetalist[[i,1]]] + ], + {i,Length[thetalist]} + ]; + Return[T] +] + + +FKinSpace[M_,Slist_,thetalist_]:=Module[ + {n,T}, + T=M; + Do[ + T=MatrixExp6[ + VecTose3[{Slist[[;;,i]]}\[Transpose]*thetalist[[i,1]]] + ].T, + {i,Length[thetalist],1,-1} + ]; + Return[T] +] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 5: VELOCITY KINEMATICS AND STATICS*) + + +JacobianBody[Blist_,thetalist_]:=Module[ + {Jb=Blist,T=IdentityMatrix[4]}, + Do[ + T=T.MatrixExp6[VecTose3[{Blist[[;;,i+1]]}\[Transpose]* + -thetalist[[i+1,1]]]]; + Jb[[;;,i]]=Flatten[Adjoint[T].{Blist[[;;,i]]}\[Transpose]], + {i,Length[thetalist]-1,1,-1} + ]; + Return[Jb] +] + + +JacobianSpace[Slist_,thetalist_]:=Module[ + {Js=Slist,T=IdentityMatrix[4]}, + Do[ + T=T.MatrixExp6[VecTose3[{Slist[[;;,i-1]]}\[Transpose]* + thetalist[[i-1,1]]]]; + Js[[;;,i]]=Flatten[Adjoint[T].{Slist[[;;,i]]}\[Transpose]], + {i,2,Length[thetalist]} + ]; + Return[Js] +] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 6: INVERSE KINEMATICS*) + + +IKinBody[Blist_,M_,T_,thetalist0_,eomg_,ev_]:=Module[ + {Vb,thetalist=thetalist0,err,i=0,maxiterations=20}, + Vb = se3ToVec[MatrixLog6[TransInv[FKinBody[M,Blist,thetalist]].T]]; + err=Norm[Vb[[1;;3,1]]]>eomg || Norm[Vb[[4;;6,1]]]>ev; + While[ + err&&ieomg||Norm[Vb[[4;;6,1]]]>ev + ]; + Return[{thetalist, !err}] +] + + +IKinSpace[Slist_,M_,T_,thetalist0_,eomg_,ev_]:=Module[ + {Tsb,Vs,thetalist=thetalist0,err,i=0,maxiterations=20}, + Tsb=FKinSpace[M,Slist,thetalist]; + Vs = Adjoint[Tsb].se3ToVec[MatrixLog6[TransInv[Tsb].T]]; + err=Norm[Vs[[1;;3,1]]]>eomg||Norm[Vs[[4;;6,1]]]>ev; + While[ + err&&ieomg || Norm[Vs[[4;;6,1]]]>ev + ]; + Return[{thetalist, !err}] +] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 8: DYNAMICS OF OPEN CHAINS*) + + +ad[V_]:=Module[ + {w}, + w=VecToso3[V[[1;;3]]]; + Return[ArrayFlatten[{{w,0},{VecToso3[V[[4;;6]]],w}}]] +] + + +InverseDynamics[thetalist_,dthetalist_,ddthetalist_,g_,Ftip_,Mlist_,Glist_,Slist_]:=Module[ + {n ,Ai,AdTi,Ti,Vi,Vdi,Fi=Ftip,taulist,Mi=IdentityMatrix[4]}, + n=Length[thetalist]; + Ai=ConstantArray[0,{6,n}]; + AdTi=ConstantArray[Null,{1,n+1}]; + Vi=ConstantArray[0,{6,n+1}]; + Vdi=ConstantArray[0,{6,n+1}]; + Vdi[[4;;6,1]]=Flatten[-g]; + AdTi[[1,n+1]]=Adjoint[TransInv[Mlist[[n+1]]]]; + taulist=ConstantArray[0,{n,1}]; + Do[ + Mi=Mi.Mlist[[i]]; + Ai[[;;,i]]=Adjoint[TransInv[Mi]].Slist[[;;,i]]; + AdTi[[1,i]]=Adjoint[MatrixExp6[ + VecTose3[{Ai[[;;,i]]}\[Transpose]*-thetalist[[i,1]]] + ].TransInv[Mlist[[i]]]]; + Vi[[;;,i+1]]=AdTi[[1,i]].Vi[[;;,i]]+ + {Ai[[;;,i]]}\[Transpose]*dthetalist[[i,1]]; + Vdi[[;;,i+1]]=AdTi[[1,i]].Vdi[[;;,i]]+ + {Ai[[;;,i]]}\[Transpose]*ddthetalist[[i,1]]+ + ad[Vi[[;;,i+1]]].Ai[[;;,i]]*dthetalist[[i,1]], + {i,1,n} + ]; + Do[ + Fi=AdTi[[1,i+1]]\[Transpose].Fi+Glist[[i]].Vdi[[;;,i+1]]- + ad[Vi[[;;,i+1]]]\[Transpose].(Glist[[i]].Vi[[;;,i+1]]); + taulist[[i,1]]=(Fi\[Transpose].Ai[[;;,i]])[[1]], + {i,n,1,-1} + ]; + Return[taulist] +] + + +MassMatrix[thetalist_,Mlist_,Glist_,Slist_]:=Module[ + {n,dthetalist,ddthetalist,g,Ftip,taulist,M}, + n=Length[thetalist]; + M=ConstantArray[0,{n,n}]; + Do[ + ddthetalist=ConstantArray[0,{n,1}]; + ddthetalist[[i,1]]=1; + M[[;;,i]]=Flatten[InverseDynamics[thetalist,ConstantArray[0,{6,n}], + ddthetalist,{{0,0,0}}\[Transpose], + {{0,0,0,0,0,0}}\[Transpose],Mlist, + Glist,Slist]], + {i,n} + ]; + Return[M] +] + + +VelQuadraticForces[thetalist_,dthetalist_,Mlist_,Glist_,Slist_]:= + InverseDynamics[thetalist,dthetalist, + ConstantArray[0,{Length[thetalist],1}], + {{0,0,0}}\[Transpose],{{0,0,0,0,0,0}}\[Transpose],Mlist, + Glist,Slist] + + +GravityForces[thetalist_,g_,Mlist_,Glist_,Slist_]:=Module[ + {n=Length[thetalist]}, + Return[InverseDynamics[ + thetalist,ConstantArray[0,{n,1}],ConstantArray[0,{n,1}],g, + {{0,0,0,0,0,0}}\[Transpose],Mlist,Glist,Slist + ]] +] + + +EndEffectorForces[thetalist_,Ftip_,Mlist_,Glist_,Slist_]:=Module[ + {n=Length[thetalist]}, + Return[InverseDynamics[ + thetalist,ConstantArray[0,{n,1}],ConstantArray[0,{n,1}], + {{0,0,0}}\[Transpose],Ftip,Mlist,Glist,Slist + ]] +] + + +ForwardDynamics[thetalist_,dthetalist_,taulist_,g_,Ftip_,Mlist_,Glist_,Slist_]:= + Inverse[MassMatrix[thetalist,Mlist,Glist,Slist]]. + (taulist-VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist]- + GravityForces[thetalist,g,Mlist,Glist,Slist]- + EndEffectorForces[thetalist,Ftip,Mlist,Glist,Slist]) + + +EulerStep[thetalist_,dthetalist_,ddthetalist_,dt_]:= + {thetalist+dt*dthetalist,dthetalist+dt*ddthetalist} + + +InverseDynamicsTrajectory[thetamat_,dthetamat_,ddthetamat_,g_,Ftipmat_,Mlist_,Glist_,Slist_]:=Module[ + {thetamatt=thetamat\[Transpose],dthetamatt=dthetamat\[Transpose], + ddthetamatt=ddthetamat\[Transpose],Ftipmatt=Ftipmat\[Transpose],taumat}, + taumat=thetamatt; + Do[ + taumat[[;;,i]]=InverseDynamics[{thetamatt[[;;,i]]}\[Transpose], + {dthetamatt[[;;,i]]}\[Transpose], + {ddthetamatt[[;;,i]]}\[Transpose],g, + Ftipmatt[[;;,i]],Mlist,Glist,Slist], + {i,Dimensions[thetamatt][[2]]} + ]; + taumat=taumat\[Transpose]; + Return[taumat] +] + + +ForwardDynamicsTrajectory[thetalist_,dthetalist_,taumat_,g_,Ftipmat_,Mlist_,Glist_,Slist_,dt_,intRes_]:=Module[ + {taumatt=taumat\[Transpose],Ftipmatt=Ftipmat\[Transpose],thetamat, + dthetamat,ddthetanext,thetanext=thetalist,dthetanext=dthetalist}, + thetamat=taumatt; + dthetamat=taumatt; + thetamat[[;;,1]]=thetanext; + dthetamat[[;;,1]]=dthetanext; + Do[ + Do[ + ddthetanext=ForwardDynamics[thetanext,dthetanext,taumatt[[;;,i]], + g,Ftipmatt[[;;,i]],Mlist,Glist, + Slist]; + {thetanext,dthetanext}=EulerStep[thetanext,dthetanext, + ddthetanext,dt/intRes], + intRes + ]; + thetamat[[;;,i+1]]=thetanext; + dthetamat[[;;,i+1]]=dthetanext, + {i,Dimensions[taumatt][[2]]-1} + ]; + thetamat=thetamat\[Transpose]; + dthetamat=dthetamat\[Transpose]; + Return[{thetamat,dthetamat}] +] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 9: TRAJECTORY GENERATION*) + + +CubicTimeScaling[Tf_,t_]:=3*(t/Tf)^2-2*(t/Tf)^3 + + +QuinticTimeScaling[Tf_,t_]:=10*(t/Tf)^3-15*(t/Tf)^4+6*(t/Tf)^5 + + +JointTrajectory[thetastart_,thetaend_,Tf_,Num_,method_]:=Module[ + {Numb=Round[Num],timegap,traj,s}, + timegap=Tf/(Numb-1); + traj= ConstantArray[0,{Length[thetastart],Numb}]; + Do[ + If[method == 3,s=CubicTimeScaling[Tf,(i-1)*timegap], + s=QuinticTimeScaling[Tf,(i-1)*timegap]]; + traj[[;;,i]]=Flatten[thetastart +s*(thetaend-thetastart)], + {i,Numb} + ]; + traj=traj\[Transpose]; + Return[traj] +] + + +ScrewTrajectory[Xstart_,Xend_,Tf_,N_,method_]:=Module[ + {timegap=Tf/(N-1),traj=ConstantArray[Null,N],s}, + Do[ + If[method==3,s=CubicTimeScaling[Tf,(i-1)*timegap], + s=QuinticTimeScaling[Tf,(i-1)*timegap]]; + traj[[i]]=Xstart.MatrixExp6[MatrixLog6[TransInv[Xstart].Xend]*s], + {i,N} + ]; + Return[traj] +] + + +CartesianTrajectory[Xstart_,Xend_,Tf_,N_,method_]:=Module[ + {timegap=Tf/(N-1),traj=ConstantArray[Null,N],s,Rstart,pstart,Rend,pend}, + {Rstart,pstart}=TransToRp[Xstart];{Rend,pend}=TransToRp[Xend]; + Do[ + If[ + method == 3, + s=CubicTimeScaling[Tf,(i-1)*timegap], + s=QuinticTimeScaling[Tf,(i-1)*timegap] + ]; + traj[[i]]=ArrayFlatten[ + {{Rstart.MatrixExp3[MatrixLog3[Rstart\[Transpose].Rend]*s], + pstart+s*(pend-pstart)}, + {0,1}} + ], + {i,N} + ]; + Return[traj] +] + + +(* ::Subsection::Closed:: *) +(*CHAPTER 11: ROBOT CONTROL*) + + +ComputedTorque[thetalist_,dthetalist_,eint_,g_,Mlist_,Glist_,Slist_,thetalistd_,dthetalistd_,ddthetalistd_,Kp_,Ki_,Kd_]:=Module[ + {e=thetalistd-thetalist}, + Return[ + MassMatrix[thetalist,Mlist,Glist,Slist]. + (Kp*e+Ki*(eint+e)+Kd*(dthetalistd-dthetalist))+ + InverseDynamics[thetalist,dthetalist,ddthetalistd,g, + {{0,0,0,0,0,0}}\[Transpose],Mlist,Glist,Slist] + ] +] + + +SimulateControl[thetalist_,dthetalist_,g_,Ftipmat_,Mlist_,Glist_,Slist_,thetamatd_,dthetamatd_,ddthetamatd_,gtilde_,Mtildelist_,Gtildelist_,Kp_,Ki_,Kd_,dt_,intRes_]:=Module[ + {Ftipmatt=Ftipmat\[Transpose],thetamatdt=thetamatd\[Transpose], + dthetamatdt=dthetamatd\[Transpose],ddthetamatdt=ddthetamatd\[Transpose], + taumat,thetamat,n,thetacurrent,dthetacurrent,eint,taulist,ddthetalist, + links, plotAngles, labels,style,col}, + n=Dimensions[thetamatdt][[2]]; + thetamat=thetamatdt; + taumat=thetamatdt; + thetacurrent=thetalist; + dthetacurrent=dthetalist; + eint=ConstantArray[0,{Dimensions[thetamatdt][[1]],1}]; + Do[ + taulist= + ComputedTorque[thetacurrent,dthetacurrent,eint,gtilde,Mtildelist, + Gtildelist,Slist,{thetamatdt[[;;,i]]}\[Transpose], + {dthetamatdt[[;;,i]]}\[Transpose], + {ddthetamatdt[[;;,i]]}\[Transpose],Kp,Ki,Kd]; + Do[ + ddthetalist=ForwardDynamics[thetacurrent,dthetacurrent,taulist,g, + {Ftipmatt[[;;,i]]}\[Transpose],Mlist, + Glist,Slist]; + {thetacurrent,dthetacurrent}= + EulerStep[thetacurrent,dthetacurrent,ddthetalist, + (dt/intRes)], + {j,intRes} + ]; + taumat[[;;,i]]=taulist; + thetamat[[;;,i]]=thetacurrent; + eint=eint+(dt*(thetamatdt[[;;,i]]-thetacurrent)), + {i,n} + ]; + (*Output plotting*) + links=Dimensions[thetamat][[1]]; + plotAngles ={}; + labels = {}; + style={}; + Do[AppendTo[plotAngles, Flatten[thetamat[[i,;;]]]]; + AppendTo[plotAngles, Flatten[thetamatdt[[i,;;]]]]; + AppendTo[labels, StringJoin["ActualTheta",ToString[i]]]; + AppendTo[labels, StringJoin["DesiredTheta",ToString[i]]]; + col = RandomColor[]; + AppendTo[style, col]; + AppendTo[style, {col,Dashed}],{i,1,links}]; + Print[ListLinePlot[plotAngles,PlotLegends->labels,PlotStyle->style, + AxesLabel->{"Time","Joint Angles"}, + PlotLabel->"Plot of Actual and Desired Joint Angles", + PlotRange->Full]]; + taumat=taumat\[Transpose]; + thetamat=thetamat\[Transpose]; + Return[{taumat,thetamat}] +] + + +(* ::Section::Closed:: *) +(*END*) + + +End[]; + + +Protect["NearZero"]; +Protect["RotInv","VecToso3","so3ToVec","AxisAng3","MatrixExp3","MatrixLog3", + "DistanceToSO3","TestIfSO3","ProjectToSO3","RpToTrans","TransToRp", + "TransInv","VecTose3","se3ToVec","Adjoint","ScrewToAxis","AxisAng6", + "MatrixExp6","MatrixLog6","DistanceToSE3","TestIfSE3", + "ProjectToSE3"]; +Protect["FKinBody","FKinSpace"]; +Protect["JacobianBody","JacobianSpace"]; +Protect["IKinBody","IKinSpace"]; +Protect["ad","InverseDynamics","MassMatrix","VelQuadraticForces", + "GravityForces","EndEffectorForces","ForwardDynamics","EulerStep", + "InverseDynamicsTrajectory","ForwardDynamicsTrajectory"]; +Protect["CubicTimeScaling","QuinticTimeScaling","JointTrajectory", + "ScrewTrajectory","CartesianTrajectory"]; +Protect["ComputedTorque","SimulateControl"]; + + +EndPackage[]; diff --git a/packages/Mathematica/README.md b/packages/Mathematica/README.md new file mode 100644 index 0000000..ca90a7c --- /dev/null +++ b/packages/Mathematica/README.md @@ -0,0 +1,97 @@ +# "ModernRobotics" Mathematica Package Instructions # + +## Installing the Package ## + +### Recommended Installation Instructions ### + +If you'd like to be able to use this package inside of any notebook, +regardless of the notebook's location on your filesystem, then you can use +Mathematica's front end to install this package. Use the following steps: + +1. Download this package +2. Click `File -> Install...` +3. Select `Package` for the _Type of Item to Install_ +4. Chose `From File...` for the _Source_ +5. Navigate to `ModernRobotics.m` and select it as the source for the + package +6. The _Install Name_ should default to `ModernRobotics`; if it doesn't then + fill in the install name to be `ModernRobotics` +7. Choose whether you want the package installed for a single user or for all + users (may require administrative privileges) +8. Click `OK` + +### Manual Installation Instructions ### + +If you have any troubles with the front-end installation described above, try +manually installing the package by copying the `ModernRobotics.m` file into +the following directory. + +```sh +$MATHPATH/Applications/ModernRobotics/ +``` + +The value of `$MATHPATH` will depend on your system. You can determine it by +looking at the `$Path` variable inside Mathematica. Here are some paths for +common operating systems: + ++ OS-X: `~/Library/Mathematica/` ++ Linux (Debian): `~/.Mathematica/` ++ Windows: `%APPDATA%\Mathematica\` + +## Loading the Package ## + +After installing the package, load the library from any notebook by running + +``` +<"] +``` + +If the notebook and the package are located in the same directory you could +use + +``` +SetDirectory[NotebookDirectory[]] +``` + +Note that since the package is not installed, you need to set the directory +shown above in every notebook in which this package is used. Loading is still +required before using. + +## Uninstalling the Package ## + +To uninstall the package, delete the file `ModernRobotics.wl` or +`ModernRobotics.m` in the following directory. + +```sh +$MATHPATH/Applications/ModernRobotics/ +``` \ No newline at end of file diff --git a/packages/Python/README.md b/packages/Python/README.md new file mode 100644 index 0000000..6ae63a3 --- /dev/null +++ b/packages/Python/README.md @@ -0,0 +1,75 @@ +# "modern_robotics" Python Package Instructions # + +## Dependency Requirement + +`numpy` should be preinstalled. + +## Installing the Package ## + +### Recommended Method ### + +Use [pip](https://en.wikipedia.org/wiki/Pip_(package_manager)) to install by +running + +``` +pip install modern_robotics +``` + +If pip is not preinstalled, check +[here](https://pip.pypa.io/en/stable/installing/) for help installing pip. + +### Alternative Method ### + +Download the package and run `python setup.py build` and +`python setup.py install` in the package directory + +## Importing the Package ## + +To import the package, we recommend using + +``` +import modern_robotics as mr +``` + +This process is required for any Python program using this package. + +## Using the Package ## + +After importing the package, you should be able to use any function in the +package. Taking the function `RotInv` for example, you can check the +description and usage example of this function by running + +``` +help(mr.RotInv) +``` + +As mentioned in the function usage example, you can try using this function +by running + +``` +R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) +invR = mr.RotInv(R) +``` + +You should get the variable `invR` whose value is the same as the output +shown in the function usage example. + +Your complete first test code that will return invR could be as follows: +``` +import modern_robotics as mr +import numpy as np +R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) +invR = mr.RotInv(R) +print(invR) +``` + +## Using the Package Locally ## + +It is possible to use the package locally without installation. Download and +place the package in the working directory. Note that since the package is +not installed, you need to move the package if the working directory is +changed. Importing is still required before using. diff --git a/packages/Python/modern_robotics/__init__.py b/packages/Python/modern_robotics/__init__.py new file mode 100644 index 0000000..1c4a7c8 --- /dev/null +++ b/packages/Python/modern_robotics/__init__.py @@ -0,0 +1,4 @@ +from .__version__ import __version__ + +from .core import * + diff --git a/packages/Python/modern_robotics/__version__.py b/packages/Python/modern_robotics/__version__.py new file mode 100644 index 0000000..d930acc --- /dev/null +++ b/packages/Python/modern_robotics/__version__.py @@ -0,0 +1,2 @@ + +__version__ = '1.1.0' diff --git a/packages/Python/modern_robotics/core.py b/packages/Python/modern_robotics/core.py new file mode 100644 index 0000000..b489b6c --- /dev/null +++ b/packages/Python/modern_robotics/core.py @@ -0,0 +1,1899 @@ +from __future__ import print_function +''' +*************************************************************************** +Modern Robotics: Mechanics, Planning, and Control. +Code Library +*************************************************************************** +Author: Huan Weng, Bill Hunt, Jarvis Schultz, Mikhail Todes, +Email: huanweng@u.northwestern.edu +Date: January 2018 +*************************************************************************** +Language: Python +Also available in: MATLAB, Mathematica +Required library: numpy +Optional library: matplotlib +*************************************************************************** +''' + +''' +*** IMPORTS *** +''' + +import numpy as np + +''' +*** BASIC HELPER FUNCTIONS *** +''' + +def NearZero(z): + """Determines whether a scalar is small enough to be treated as zero + + :param z: A scalar input to check + :return: True if z is close to zero, false otherwise + + Example Input: + z = -1e-7 + Output: + True + """ + return abs(z) < 1e-6 + +def Normalize(V): + """Normalizes a vector + + :param V: A vector + :return: A unit vector pointing in the same direction as z + + Example Input: + V = np.array([1, 2, 3]) + Output: + np.array([0.26726124, 0.53452248, 0.80178373]) + """ + return V / np.linalg.norm(V) + +''' +*** CHAPTER 3: RIGID-BODY MOTIONS *** +''' + +def RotInv(R): + """Inverts a rotation matrix + + :param R: A rotation matrix + :return: The inverse of R + + Example Input: + R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) + Output: + np.array([[0, 1, 0], + [0, 0, 1], + [1, 0, 0]]) + """ + return np.array(R).T + +def VecToso3(omg): + """Converts a 3-vector to an so(3) representation + + :param omg: A 3-vector + :return: The skew symmetric representation of omg + + Example Input: + omg = np.array([1, 2, 3]) + Output: + np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + """ + return np.array([[0, -omg[2], omg[1]], + [omg[2], 0, -omg[0]], + [-omg[1], omg[0], 0]]) + +def so3ToVec(so3mat): + """Converts an so(3) representation to a 3-vector + + :param so3mat: A 3x3 skew-symmetric matrix + :return: The 3-vector corresponding to so3mat + + Example Input: + so3mat = np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + Output: + np.array([1, 2, 3]) + """ + return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]]) + +def AxisAng3(expc3): + """Converts a 3-vector of exponential coordinates for rotation into + axis-angle form + + :param expc3: A 3-vector of exponential coordinates for rotation + :return omghat: A unit rotation axis + :return theta: The corresponding rotation angle + + Example Input: + expc3 = np.array([1, 2, 3]) + Output: + (np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413) + """ + return (Normalize(expc3), np.linalg.norm(expc3)) + +def MatrixExp3(so3mat): + """Computes the matrix exponential of a matrix in so(3) + + :param so3mat: A 3x3 skew-symmetric matrix + :return: The matrix exponential of so3mat + + Example Input: + so3mat = np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + Output: + np.array([[-0.69492056, 0.71352099, 0.08929286], + [-0.19200697, -0.30378504, 0.93319235], + [ 0.69297817, 0.6313497 , 0.34810748]]) + """ + omgtheta = so3ToVec(so3mat) + if NearZero(np.linalg.norm(omgtheta)): + return np.eye(3) + else: + theta = AxisAng3(omgtheta)[1] + omgmat = so3mat / theta + return np.eye(3) + np.sin(theta) * omgmat \ + + (1 - np.cos(theta)) * np.dot(omgmat, omgmat) + +def MatrixLog3(R): + """Computes the matrix logarithm of a rotation matrix + + :param R: A 3x3 rotation matrix + :return: The matrix logarithm of R + + Example Input: + R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) + Output: + np.array([[ 0, -1.20919958, 1.20919958], + [ 1.20919958, 0, -1.20919958], + [-1.20919958, 1.20919958, 0]]) + """ + acosinput = (np.trace(R) - 1) / 2.0 + if acosinput >= 1: + return np.zeros((3, 3)) + elif acosinput <= -1: + if not NearZero(1 + R[2][2]): + omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \ + * np.array([R[0][2], R[1][2], 1 + R[2][2]]) + elif not NearZero(1 + R[1][1]): + omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \ + * np.array([R[0][1], 1 + R[1][1], R[2][1]]) + else: + omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \ + * np.array([1 + R[0][0], R[1][0], R[2][0]]) + return VecToso3(np.pi * omg) + else: + theta = np.arccos(acosinput) + return theta / 2.0 / np.sin(theta) * (R - np.array(R).T) + +def RpToTrans(R, p): + """Converts a rotation matrix and a position vector into homogeneous + transformation matrix + + :param R: A 3x3 rotation matrix + :param p: A 3-vector + :return: A homogeneous transformation matrix corresponding to the inputs + + Example Input: + R = np.array([[1, 0, 0], + [0, 0, -1], + [0, 1, 0]]) + p = np.array([1, 2, 5]) + Output: + np.array([[1, 0, 0, 1], + [0, 0, -1, 2], + [0, 1, 0, 5], + [0, 0, 0, 1]]) + """ + return np.r_[np.c_[R, p], [[0, 0, 0, 1]]] + +def TransToRp(T): + """Converts a homogeneous transformation matrix into a rotation matrix + and position vector + + :param T: A homogeneous transformation matrix + :return R: The corresponding rotation matrix, + :return p: The corresponding position vector. + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + (np.array([[1, 0, 0], + [0, 0, -1], + [0, 1, 0]]), + np.array([0, 0, 3])) + """ + T = np.array(T) + return T[0: 3, 0: 3], T[0: 3, 3] + +def TransInv(T): + """Inverts a homogeneous transformation matrix + + :param T: A homogeneous transformation matrix + :return: The inverse of T + Uses the structure of transformation matrices to avoid taking a matrix + inverse, for efficiency. + + Example input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[1, 0, 0, 0], + [0, 0, 1, -3], + [0, -1, 0, 0], + [0, 0, 0, 1]]) + """ + R, p = TransToRp(T) + Rt = np.array(R).T + return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]] + +def VecTose3(V): + """Converts a spatial velocity vector into a 4x4 matrix in se3 + + :param V: A 6-vector representing a spatial velocity + :return: The 4x4 se3 representation of V + + Example Input: + V = np.array([1, 2, 3, 4, 5, 6]) + Output: + np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], + [ 0, 0, 0, 0]]) + """ + return np.r_[np.c_[VecToso3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]], + np.zeros((1, 4))] + +def se3ToVec(se3mat): + """ Converts an se3 matrix into a spatial velocity vector + + :param se3mat: A 4x4 matrix in se3 + :return: The spatial velocity 6-vector corresponding to se3mat + + Example Input: + se3mat = np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], + [ 0, 0, 0, 0]]) + Output: + np.array([1, 2, 3, 4, 5, 6]) + """ + return np.r_[[se3mat[2][1], se3mat[0][2], se3mat[1][0]], + [se3mat[0][3], se3mat[1][3], se3mat[2][3]]] + +def Adjoint(T): + """Computes the adjoint representation of a homogeneous transformation + matrix + + :param T: A homogeneous transformation matrix + :return: The 6x6 adjoint representation [AdT] of T + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[1, 0, 0, 0, 0, 0], + [0, 0, -1, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 3, 1, 0, 0], + [3, 0, 0, 0, 0, -1], + [0, 0, 0, 0, 1, 0]]) + """ + R, p = TransToRp(T) + return np.r_[np.c_[R, np.zeros((3, 3))], + np.c_[np.dot(VecToso3(p), R), R]] + +def ScrewToAxis(q, s, h): + """Takes a parametric description of a screw axis and converts it to a + normalized screw axis + + :param q: A point lying on the screw axis + :param s: A unit vector in the direction of the screw axis + :param h: The pitch of the screw axis + :return: A normalized screw axis described by the inputs + + Example Input: + q = np.array([3, 0, 0]) + s = np.array([0, 0, 1]) + h = 2 + Output: + np.array([0, 0, 1, 0, -3, 2]) + """ + return np.r_[s, np.cross(q, s) + np.dot(h, s)] + +def AxisAng6(expc6): + """Converts a 6-vector of exponential coordinates into screw axis-angle + form + + :param expc6: A 6-vector of exponential coordinates for rigid-body motion + S*theta + :return S: The corresponding normalized screw axis + :return theta: The distance traveled along/about S + + Example Input: + expc6 = np.array([1, 0, 0, 1, 2, 3]) + Output: + (np.array([1.0, 0.0, 0.0, 1.0, 2.0, 3.0]), 1.0) + """ + theta = np.linalg.norm([expc6[0], expc6[1], expc6[2]]) + if NearZero(theta): + theta = np.linalg.norm([expc6[3], expc6[4], expc6[5]]) + return (np.array(expc6 / theta), theta) + +def MatrixExp6(se3mat): + """Computes the matrix exponential of an se3 representation of + exponential coordinates + + :param se3mat: A matrix in se3 + :return: The matrix exponential of se3mat + + Example Input: + se3mat = np.array([[0, 0, 0, 0], + [0, 0, -1.57079632, 2.35619449], + [0, 1.57079632, 0, 2.35619449], + [0, 0, 0, 0]]) + Output: + np.array([[1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, -1.0, 0.0], + [0.0, 1.0, 0.0, 3.0], + [ 0, 0, 0, 1]]) + """ + se3mat = np.array(se3mat) + omgtheta = so3ToVec(se3mat[0: 3, 0: 3]) + if NearZero(np.linalg.norm(omgtheta)): + return np.r_[np.c_[np.eye(3), se3mat[0: 3, 3]], [[0, 0, 0, 1]]] + else: + theta = AxisAng3(omgtheta)[1] + omgmat = se3mat[0: 3, 0: 3] / theta + return np.r_[np.c_[MatrixExp3(se3mat[0: 3, 0: 3]), + np.dot(np.eye(3) * theta \ + + (1 - np.cos(theta)) * omgmat \ + + (theta - np.sin(theta)) \ + * np.dot(omgmat,omgmat), + se3mat[0: 3, 3]) / theta], + [[0, 0, 0, 1]]] + +def MatrixLog6(T): + """Computes the matrix logarithm of a homogeneous transformation matrix + + :param R: A matrix in SE3 + :return: The matrix logarithm of R + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[0, 0, 0, 0] + [0, 0, -1.57079633, 2.35619449] + [0, 1.57079633, 0, 2.35619449] + [0, 0, 0, 0]]) + """ + R, p = TransToRp(T) + omgmat = MatrixLog3(R) + if np.array_equal(omgmat, np.zeros((3, 3))): + return np.r_[np.c_[np.zeros((3, 3)), + [T[0][3], T[1][3], T[2][3]]], + [[0, 0, 0, 0]]] + else: + theta = np.arccos((np.trace(R) - 1) / 2.0) + return np.r_[np.c_[omgmat, + np.dot(np.eye(3) - omgmat / 2.0 \ + + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \ + * np.dot(omgmat,omgmat) / theta,[T[0][3], + T[1][3], + T[2][3]])], + [[0, 0, 0, 0]]] + +def ProjectToSO3(mat): + """Returns a projection of mat into SO(3) + + :param mat: A matrix near SO(3) to project to SO(3) + :return: The closest matrix to R that is in SO(3) + Projects a matrix mat to the closest matrix in SO(3) using singular-value + decomposition (see + http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + This function is only appropriate for matrices close to SO(3). + + Example Input: + mat = np.array([[ 0.675, 0.150, 0.720], + [ 0.370, 0.771, -0.511], + [-0.630, 0.619, 0.472]]) + Output: + np.array([[ 0.67901136, 0.14894516, 0.71885945], + [ 0.37320708, 0.77319584, -0.51272279], + [-0.63218672, 0.61642804, 0.46942137]]) + """ + U, s, Vh = np.linalg.svd(mat) + R = np.dot(U, Vh) + if np.linalg.det(R) < 0: + # In this case the result may be far from mat. + R[:, 2] = -R[:, 2] + return R + +def ProjectToSE3(mat): + """Returns a projection of mat into SE(3) + + :param mat: A 4x4 matrix to project to SE(3) + :return: The closest matrix to T that is in SE(3) + Projects a matrix mat to the closest matrix in SE(3) using singular-value + decomposition (see + http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + This function is only appropriate for matrices close to SE(3). + + Example Input: + mat = np.array([[ 0.675, 0.150, 0.720, 1.2], + [ 0.370, 0.771, -0.511, 5.4], + [-0.630, 0.619, 0.472, 3.6], + [ 0.003, 0.002, 0.010, 0.9]]) + Output: + np.array([[ 0.67901136, 0.14894516, 0.71885945, 1.2 ], + [ 0.37320708, 0.77319584, -0.51272279, 5.4 ], + [-0.63218672, 0.61642804, 0.46942137, 3.6 ], + [ 0. , 0. , 0. , 1. ]]) + """ + mat = np.array(mat) + return RpToTrans(ProjectToSO3(mat[:3, :3]), mat[:3, 3]) + +def DistanceToSO3(mat): + """Returns the Frobenius norm to describe the distance of mat from the + SO(3) manifold + + :param mat: A 3x3 matrix + :return: A quantity describing the distance of mat from the SO(3) + manifold + Computes the distance from mat to the SO(3) manifold using the following + method: + If det(mat) <= 0, return a large number. + If det(mat) > 0, return norm(mat^T.mat - I). + + Example Input: + mat = np.array([[ 1.0, 0.0, 0.0 ], + [ 0.0, 0.1, -0.95], + [ 0.0, 1.0, 0.1 ]]) + Output: + 0.08835 + """ + if np.linalg.det(mat) > 0: + return np.linalg.norm(np.dot(np.array(mat).T, mat) - np.eye(3)) + else: + return 1e+9 + +def DistanceToSE3(mat): + """Returns the Frobenius norm to describe the distance of mat from the + SE(3) manifold + + :param mat: A 4x4 matrix + :return: A quantity describing the distance of mat from the SE(3) + manifold + Computes the distance from mat to the SE(3) manifold using the following + method: + Compute the determinant of matR, the top 3x3 submatrix of mat. + If det(matR) <= 0, return a large number. + If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T.matR, + and set the first three entries of the fourth column of mat to zero. Then + return norm(mat - I). + + Example Input: + mat = np.array([[ 1.0, 0.0, 0.0, 1.2 ], + [ 0.0, 0.1, -0.95, 1.5 ], + [ 0.0, 1.0, 0.1, -0.9 ], + [ 0.0, 0.0, 0.1, 0.98 ]]) + Output: + 0.134931 + """ + matR = np.array(mat)[0: 3, 0: 3] + if np.linalg.det(matR) > 0: + return np.linalg.norm(np.r_[np.c_[np.dot(np.transpose(matR), matR), + np.zeros((3, 1))], + [np.array(mat)[3, :]]] - np.eye(4)) + else: + return 1e+9 + +def TestIfSO3(mat): + """Returns true if mat is close to or on the manifold SO(3) + + :param mat: A 3x3 matrix + :return: True if mat is very close to or in SO(3), false otherwise + Computes the distance d from mat to the SO(3) manifold using the + following method: + If det(mat) <= 0, d = a large number. + If det(mat) > 0, d = norm(mat^T.mat - I). + If d is close to zero, return true. Otherwise, return false. + + Example Input: + mat = np.array([[1.0, 0.0, 0.0 ], + [0.0, 0.1, -0.95], + [0.0, 1.0, 0.1 ]]) + Output: + False + """ + return abs(DistanceToSO3(mat)) < 1e-3 + +def TestIfSE3(mat): + """Returns true if mat is close to or on the manifold SE(3) + + :param mat: A 4x4 matrix + :return: True if mat is very close to or in SE(3), false otherwise + Computes the distance d from mat to the SE(3) manifold using the + following method: + Compute the determinant of the top 3x3 submatrix of mat. + If det(mat) <= 0, d = a large number. + If det(mat) > 0, replace the top 3x3 submatrix of mat with mat^T.mat, and + set the first three entries of the fourth column of mat to zero. + Then d = norm(T - I). + If d is close to zero, return true. Otherwise, return false. + + Example Input: + mat = np.array([[1.0, 0.0, 0.0, 1.2], + [0.0, 0.1, -0.95, 1.5], + [0.0, 1.0, 0.1, -0.9], + [0.0, 0.0, 0.1, 0.98]]) + Output: + False + """ + return abs(DistanceToSE3(mat)) < 1e-3 + +''' +*** CHAPTER 4: FORWARD KINEMATICS *** +''' + +def FKinBody(M, Blist, thetalist): + """Computes forward kinematics in the body frame for an open chain robot + + :param M: The home configuration (position and orientation) of the end- + effector + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: A homogeneous transformation matrix representing the end- + effector frame when the joints are at the specified coordinates + (i.t.o Body Frame) + + Example Input: + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + Blist = np.array([[0, 0, -1, 2, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0.1]]).T + thetalist = np.array([np.pi / 2.0, 3, np.pi]) + Output: + np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.68584073], + [0, 0, 0, 1]]) + """ + T = np.array(M) + for i in range(len(thetalist)): + T = np.dot(T, MatrixExp6(VecTose3(np.array(Blist)[:, i] \ + * thetalist[i]))) + return T + +def FKinSpace(M, Slist, thetalist): + """Computes forward kinematics in the space frame for an open chain robot + + :param M: The home configuration (position and orientation) of the end- + effector + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: A homogeneous transformation matrix representing the end- + effector frame when the joints are at the specified coordinates + (i.t.o Space Frame) + + Example Input: + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + Slist = np.array([[0, 0, 1, 4, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, -1, -6, 0, -0.1]]).T + thetalist = np.array([np.pi / 2.0, 3, np.pi]) + Output: + np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.68584073], + [0, 0, 0, 1]]) + """ + T = np.array(M) + for i in range(len(thetalist) - 1, -1, -1): + T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \ + * thetalist[i])), T) + return T + +''' +*** CHAPTER 5: VELOCITY KINEMATICS AND STATICS*** +''' + +def JacobianBody(Blist, thetalist): + """Computes the body Jacobian for an open chain robot + + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: The body Jacobian corresponding to the inputs (6xn real + numbers) + + Example Input: + Blist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], + [1, 0, 0, 0.2, 0.3, 0.4]]).T + thetalist = np.array([0.2, 1.1, 0.1, 1.2]) + Output: + np.array([[-0.04528405, 0.99500417, 0, 1] + [ 0.74359313, 0.09304865, 0.36235775, 0] + [-0.66709716, 0.03617541, -0.93203909, 0] + [ 2.32586047, 1.66809, 0.56410831, 0.2] + [-1.44321167, 2.94561275, 1.43306521, 0.3] + [-2.06639565, 1.82881722, -1.58868628, 0.4]]) + """ + Jb = np.array(Blist).copy().astype(float) + T = np.eye(4) + for i in range(len(thetalist) - 2, -1, -1): + T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i + 1] \ + * -thetalist[i + 1]))) + Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) + return Jb + +def JacobianSpace(Slist, thetalist): + """Computes the space Jacobian for an open chain robot + + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: The space Jacobian corresponding to the inputs (6xn real + numbers) + + Example Input: + Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], + [1, 0, 0, 0.2, 0.3, 0.4]]).T + thetalist = np.array([0.2, 1.1, 0.1, 1.2]) + Output: + np.array([[ 0, 0.98006658, -0.09011564, 0.95749426] + [ 0, 0.19866933, 0.4445544, 0.28487557] + [ 1, 0, 0.89120736, -0.04528405] + [ 0, 1.95218638, -2.21635216, -0.51161537] + [0.2, 0.43654132, -2.43712573, 2.77535713] + [0.2, 2.96026613, 3.23573065, 2.22512443]]) + """ + Js = np.array(Slist).copy().astype(float) + T = np.eye(4) + for i in range(1, len(thetalist)): + T = np.dot(T, MatrixExp6(VecTose3(np.array(Slist)[:, i - 1] \ + * thetalist[i - 1]))) + Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i]) + return Js + +''' +*** CHAPTER 6: INVERSE KINEMATICS *** +''' + +def IKinBody(Blist, M, T, thetalist0, eomg, ev): + """Computes inverse kinematics in the body frame for an open chain robot + + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param M: The home configuration of the end-effector + :param T: The desired end-effector configuration Tsd + :param thetalist0: An initial guess of joint angles that are close to + satisfying Tsd + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector + orientation error less than eomg + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector + position error less than ev + :return thetalist: Joint angles that achieve T within the specified + tolerances, + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set + number of maximum iterations without finding a solution + within the tolerances eomg and ev. + Uses an iterative Newton-Raphson root-finding method. + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. + + Example Input: + Blist = np.array([[0, 0, -1, 2, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0.1]]).T + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], + [0, 0, 0, 1]]) + thetalist0 = np.array([1.5, 2.5, 3]) + eomg = 0.01 + ev = 0.001 + Output: + (np.array([1.57073819, 2.999667, 3.14153913]), True) + """ + thetalist = np.array(thetalist0).copy() + i = 0 + maxiterations = 20 + Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ + thetalist)), T))) + err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + while err and i < maxiterations: + thetalist = thetalist \ + + np.dot(np.linalg.pinv(JacobianBody(Blist, \ + thetalist)), Vb) + i = i + 1 + Vb \ + = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ + thetalist)), T))) + err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + return (thetalist, not err) + +def IKinSpace(Slist, M, T, thetalist0, eomg, ev): + """Computes inverse kinematics in the space frame for an open chain robot + + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param M: The home configuration of the end-effector + :param T: The desired end-effector configuration Tsd + :param thetalist0: An initial guess of joint angles that are close to + satisfying Tsd + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector + orientation error less than eomg + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector + position error less than ev + :return thetalist: Joint angles that achieve T within the specified + tolerances, + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set + number of maximum iterations without finding a solution + within the tolerances eomg and ev. + Uses an iterative Newton-Raphson root-finding method. + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. + + Example Input: + Slist = np.array([[0, 0, 1, 4, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, -1, -6, 0, -0.1]]).T + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], + [0, 0, 0, 1]]) + thetalist0 = np.array([1.5, 2.5, 3]) + eomg = 0.01 + ev = 0.001 + Output: + (np.array([ 1.57073783, 2.99966384, 3.1415342 ]), True) + """ + thetalist = np.array(thetalist0).copy() + i = 0 + maxiterations = 20 + Tsb = FKinSpace(M,Slist, thetalist) + Vs = np.dot(Adjoint(Tsb), \ + se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) + err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + while err and i < maxiterations: + thetalist = thetalist \ + + np.dot(np.linalg.pinv(JacobianSpace(Slist, \ + thetalist)), Vs) + i = i + 1 + Tsb = FKinSpace(M, Slist, thetalist) + Vs = np.dot(Adjoint(Tsb), \ + se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) + err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + return (thetalist, not err) + +''' +*** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +''' + +def ad(V): + """Calculate the 6x6 matrix [adV] of the given 6-vector + + :param V: A 6-vector spatial velocity + :return: The corresponding 6x6 matrix [adV] + + Used to calculate the Lie bracket [V1, V2] = [adV1]V2 + + Example Input: + V = np.array([1, 2, 3, 4, 5, 6]) + Output: + np.array([[ 0, -3, 2, 0, 0, 0], + [ 3, 0, -1, 0, 0, 0], + [-2, 1, 0, 0, 0, 0], + [ 0, -6, 5, 0, -3, 2], + [ 6, 0, -4, 3, 0, -1], + [-5, 4, 0, -2, 1, 0]]) + """ + omgmat = VecToso3([V[0], V[1], V[2]]) + return np.r_[np.c_[omgmat, np.zeros((3, 3))], + np.c_[VecToso3([V[3], V[4], V[5]]), omgmat]] + +def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ + Glist, Slist): + """Computes inverse dynamics in the space frame for an open chain robot + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param ddthetalist: n-vector of joint accelerations + :param g: Gravity vector g + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The n-vector of required joint forces/torques + This function uses forward-backward Newton-Euler iterations to solve the + equation: + taulist = Mlist(thetalist)ddthetalist + c(thetalist,dthetalist) \ + + g(thetalist) + Jtr(thetalist)Ftip + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([74.69616155, -33.06766016, -3.23057314]) + """ + n = len(thetalist) + Mi = np.eye(4) + Ai = np.zeros((6, n)) + AdTi = [[None]] * (n + 1) + Vi = np.zeros((6, n + 1)) + Vdi = np.zeros((6, n + 1)) + Vdi[:, 0] = np.r_[[0, 0, 0], -np.array(g)] + AdTi[n] = Adjoint(TransInv(Mlist[n])) + Fi = np.array(Ftip).copy() + taulist = np.zeros(n) + for i in range(n): + Mi = np.dot(Mi,Mlist[i]) + Ai[:, i] = np.dot(Adjoint(TransInv(Mi)), np.array(Slist)[:, i]) + AdTi[i] = Adjoint(np.dot(MatrixExp6(VecTose3(Ai[:, i] * \ + -thetalist[i])), \ + TransInv(Mlist[i]))) + Vi[:, i + 1] = np.dot(AdTi[i], Vi[:,i]) + Ai[:, i] * dthetalist[i] + Vdi[:, i + 1] = np.dot(AdTi[i], Vdi[:, i]) \ + + Ai[:, i] * ddthetalist[i] \ + + np.dot(ad(Vi[:, i + 1]), Ai[:, i]) * dthetalist[i] + for i in range (n - 1, -1, -1): + Fi = np.dot(np.array(AdTi[i + 1]).T, Fi) \ + + np.dot(np.array(Glist[i]), Vdi[:, i + 1]) \ + - np.dot(np.array(ad(Vi[:, i + 1])).T, \ + np.dot(np.array(Glist[i]), Vi[:, i + 1])) + taulist[i] = np.dot(np.array(Fi).T, Ai[:, i]) + return taulist + +def MassMatrix(thetalist, Mlist, Glist, Slist): + """Computes the mass matrix of an open chain robot based on the given + configuration + + :param thetalist: A list of joint variables + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The numerical inertia matrix M(thetalist) of an n-joint serial + chain at the given configuration thetalist + This function calls InverseDynamics n times, each time passing a + ddthetalist vector with a single element equal to one and all other + inputs set to zero. + Each call of InverseDynamics generates a single column, and these columns + are assembled to create the inertia matrix. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([[ 2.25433380e+01, -3.07146754e-01, -7.18426391e-03] + [-3.07146754e-01, 1.96850717e+00, 4.32157368e-01] + [-7.18426391e-03, 4.32157368e-01, 1.91630858e-01]]) + """ + n = len(thetalist) + M = np.zeros((n, n)) + for i in range (n): + ddthetalist = [0] * n + ddthetalist[i] = 1 + M[:, i] = InverseDynamics(thetalist, [0] * n, ddthetalist, \ + [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, \ + Glist, Slist) + return M + +def VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist): + """Computes the Coriolis and centripetal terms in the inverse dynamics of + an open chain robot + + :param thetalist: A list of joint variables, + :param dthetalist: A list of joint rates, + :param Mlist: List of link frames i relative to i-1 at the home position, + :param Glist: Spatial inertia matrices Gi of the links, + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns. + :return: The vector c(thetalist,dthetalist) of Coriolis and centripetal + terms for a given thetalist and dthetalist. + This function calls InverseDynamics with g = 0, Ftip = 0, and + ddthetalist = 0. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([0.26453118, -0.05505157, -0.00689132]) + """ + return InverseDynamics(thetalist, dthetalist, [0] * len(thetalist), \ + [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, Glist, \ + Slist) + +def GravityForces(thetalist, g, Mlist, Glist, Slist): + """Computes the joint forces/torques an open chain robot requires to + overcome gravity at its configuration + + :param thetalist: A list of joint variables + :param g: 3-vector for gravitational acceleration + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return grav: The joint forces/torques required to overcome gravity at + thetalist + This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and + ddthetalist = 0. + + Example Inputs (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([28.40331262, -37.64094817, -5.4415892]) + """ + n = len(thetalist) + return InverseDynamics(thetalist, [0] * n, [0] * n, g, \ + [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist) + +def EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist): + """Computes the joint forces/torques an open chain robot requires only to + create the end-effector force Ftip + + :param thetalist: A list of joint variables + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The joint forces and torques required only to create the + end-effector force Ftip + This function calls InverseDynamics with g = 0, dthetalist = 0, and + ddthetalist = 0. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([1.40954608, 1.85771497, 1.392409]) + """ + n = len(thetalist) + return InverseDynamics(thetalist, [0] * n, [0] * n, [0, 0, 0], Ftip, \ + Mlist, Glist, Slist) + +def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \ + Glist, Slist): + """Computes forward dynamics in the space frame for an open chain robot + + :param thetalist: A list of joint variables + :param dthetalist: A list of joint rates + :param taulist: An n-vector of joint forces/torques + :param g: Gravity vector g + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The resulting joint accelerations + This function computes ddthetalist by solving: + Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) \ + - g(thetalist) - Jtr(thetalist) * Ftip + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taulist = np.array([0.5, 0.6, 0.7]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([-0.97392907, 25.58466784, -32.91499212]) + """ + return np.dot(np.linalg.inv(MassMatrix(thetalist, Mlist, Glist, \ + Slist)), \ + np.array(taulist) \ + - VelQuadraticForces(thetalist, dthetalist, Mlist, \ + Glist, Slist) \ + - GravityForces(thetalist, g, Mlist, Glist, Slist) \ + - EndEffectorForces(thetalist, Ftip, Mlist, Glist, \ + Slist)) + +def EulerStep(thetalist, dthetalist, ddthetalist, dt): + """Compute the joint angles and velocities at the next timestep using from here + first order Euler integration + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param ddthetalist: n-vector of joint accelerations + :param dt: The timestep delta t + :return thetalistNext: Vector of joint variables after dt from first + order Euler integration + :return dthetalistNext: Vector of joint rates after dt from first order + Euler integration + + Example Inputs (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + dt = 0.1 + Output: + thetalistNext: + array([ 0.11, 0.12, 0.13]) + dthetalistNext: + array([ 0.3 , 0.35, 0.4 ]) + """ + return thetalist + dt * np.array(dthetalist), \ + dthetalist + dt * np.array(ddthetalist) + +def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ + Ftipmat, Mlist, Glist, Slist): + """Calculates the joint forces/torques required to move the serial chain + along the given trajectory using inverse dynamics + + :param thetamat: An N x n matrix of robot joint variables + :param dthetamat: An N x n matrix of robot joint velocities + :param ddthetamat: An N x n matrix of robot joint accelerations + :param g: Gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The N x n matrix of joint forces/torques for the specified + trajectory, where each of the N rows is the vector of joint + forces/torques at each time step + + Example Inputs (3 Link Robot): + from __future__ import print_function + import numpy as np + import modern_robotics as mr + # Create a trajectory to follow using functions from Chapter 9 + thetastart = np.array([0, 0, 0]) + thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) + Tf = 3 + N= 1000 + method = 5 + traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) + thetamat = np.array(traj).copy() + dthetamat = np.zeros((1000,3 )) + ddthetamat = np.zeros((1000, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt + ddthetamat[i + 1, :] \ + = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((N, 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + taumat \ + = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ + Ftipmat, Mlist, Glist, Slist) + # Output using matplotlib to plot the joint forces/torques + Tau1 = taumat[:, 0] + Tau2 = taumat[:, 1] + Tau3 = taumat[:, 2] + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, Tau1, label = "Tau1") + plt.plot(timestamp, Tau2, label = "Tau2") + plt.plot(timestamp, Tau3, label = "Tau3") + plt.ylim (-40, 120) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Torque") + plt.title("Plot of Torque Trajectories") + plt.show() + """ + thetamat = np.array(thetamat).T + dthetamat = np.array(dthetamat).T + ddthetamat = np.array(ddthetamat).T + Ftipmat = np.array(Ftipmat).T + taumat = np.array(thetamat).copy() + for i in range(np.array(thetamat).shape[1]): + taumat[:, i] \ + = InverseDynamics(thetamat[:, i], dthetamat[:, i], \ + ddthetamat[:, i], g, Ftipmat[:, i], Mlist, \ + Glist, Slist) + taumat = np.array(taumat).T + return taumat + +def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \ + Mlist, Glist, Slist, dt, intRes): + """Simulates the motion of a serial chain given an open-loop history of + joint forces/torques + + :param thetalist: n-vector of initial joint variables + :param dthetalist: n-vector of initial joint rates + :param taumat: An N x n matrix of joint forces/torques, where each row is + the joint effort at any time step + :param g: Gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param dt: The timestep between consecutive joint forces/torques + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an + integer value greater than or equal to 1 + :return thetamat: The N x n matrix of robot joint angles resulting from + the specified joint forces/torques + :return dthetamat: The N x n matrix of robot joint velocities + This function calls a numerical integration procedure that uses + ForwardDynamics. + + Example Inputs (3 Link Robot): + from __future__ import print_function + import numpy as np + import modern_robotics as mr + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5], + [4.31, -0.68, -5.19], [5.18, 5.63, -4.31], + [5.85, 8.17, -2.59], [5.78, 2.79, -1.7], + [4.99, -5.3, -1.19], [4.08, -9.41, 0.07], + [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((np.array(taumat).shape[0], 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.1 + intRes = 8 + thetamat,dthetamat \ + = mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \ + Ftipmat, Mlist, Glist, Slist, dt, \ + intRes) + # Output using matplotlib to plot the joint angle/velocities + theta1 = thetamat[:, 0] + theta2 = thetamat[:, 1] + theta3 = thetamat[:, 2] + dtheta1 = dthetamat[:, 0] + dtheta2 = dthetamat[:, 1] + dtheta3 = dthetamat[:, 2] + N = np.array(taumat).shape[0] + Tf = np.array(taumat).shape[0] * dt + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, theta1, label = "Theta1") + plt.plot(timestamp, theta2, label = "Theta2") + plt.plot(timestamp, theta3, label = "Theta3") + plt.plot(timestamp, dtheta1, label = "DTheta1") + plt.plot(timestamp, dtheta2, label = "DTheta2") + plt.plot(timestamp, dtheta3, label = "DTheta3") + plt.ylim (-12, 10) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Joint Angles/Velocities") + plt.title("Plot of Joint Angles and Joint Velocities") + plt.show() + """ + taumat = np.array(taumat).T + Ftipmat = np.array(Ftipmat).T + thetamat = taumat.copy().astype(float) + thetamat[:, 0] = thetalist + dthetamat = taumat.copy().astype(float) + dthetamat[:, 0] = dthetalist + for i in range(np.array(taumat).shape[1] - 1): + for j in range(intRes): + ddthetalist \ + = ForwardDynamics(thetalist, dthetalist, taumat[:, i], g, \ + Ftipmat[:, i], Mlist, Glist, Slist) + thetalist,dthetalist = EulerStep(thetalist, dthetalist, \ + ddthetalist, 1.0 * dt / intRes) + thetamat[:, i + 1] = thetalist + dthetamat[:, i + 1] = dthetalist + thetamat = np.array(thetamat).T + dthetamat = np.array(dthetamat).T + return thetamat, dthetamat + +''' +*** CHAPTER 9: TRAJECTORY GENERATION *** +''' + +def CubicTimeScaling(Tf, t): + """Computes s(t) for a cubic time scaling + + :param Tf: Total time of the motion in seconds from rest to rest + :param t: The current time t satisfying 0 < t < Tf + :return: The path parameter s(t) corresponding to a third-order + polynomial motion that begins and ends at zero velocity + + Example Input: + Tf = 2 + t = 0.6 + Output: + 0.216 + """ + return 3 * (1.0 * t / Tf) ** 2 - 2 * (1.0 * t / Tf) ** 3 + +def QuinticTimeScaling(Tf, t): + """Computes s(t) for a quintic time scaling + + :param Tf: Total time of the motion in seconds from rest to rest + :param t: The current time t satisfying 0 < t < Tf + :return: The path parameter s(t) corresponding to a fifth-order + polynomial motion that begins and ends at zero velocity and zero + acceleration + + Example Input: + Tf = 2 + t = 0.6 + Output: + 0.16308 + """ + return 10 * (1.0 * t / Tf) ** 3 - 15 * (1.0 * t / Tf) ** 4 \ + + 6 * (1.0 * t / Tf) ** 5 + +def JointTrajectory(thetastart, thetaend, Tf, N, method): + """Computes a straight-line trajectory in joint space + + :param thetastart: The initial joint variables + :param thetaend: The final joint variables + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: A trajectory as an N x n matrix, where each row is an n-vector + of joint variables at an instant in time. The first row is + thetastart and the Nth row is thetaend . The elapsed time + between each row is Tf / (N - 1) + + Example Input: + thetastart = np.array([1, 0, 0, 1, 1, 0.2, 0,1]) + thetaend = np.array([1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]) + Tf = 4 + N = 6 + method = 3 + Output: + np.array([[ 1, 0, 0, 1, 1, 0.2, 0, 1] + [1.0208, 0.052, 0.0624, 1.0104, 1.104, 0.3872, 0.0936, 1] + [1.0704, 0.176, 0.2112, 1.0352, 1.352, 0.8336, 0.3168, 1] + [1.1296, 0.324, 0.3888, 1.0648, 1.648, 1.3664, 0.5832, 1] + [1.1792, 0.448, 0.5376, 1.0896, 1.896, 1.8128, 0.8064, 1] + [ 1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]]) + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = np.zeros((len(thetastart), N)) + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[:, i] = s * np.array(thetaend) + (1 - s) * np.array(thetastart) + traj = np.array(traj).T + return traj + +def ScrewTrajectory(Xstart, Xend, Tf, N, method): + """Computes a trajectory as a list of N SE(3) matrices corresponding to + the screw motion about a space screw axis + + :param Xstart: The initial end-effector configuration + :param Xend: The final end-effector configuration + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1). The first in the list is Xstart + and the Nth is Xend + + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 3 + Output: + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[0.904, -0.25, 0.346, 0.441] + [0.346, 0.904, -0.25, 0.529] + [-0.25, 0.346, 0.904, 1.601] + [ 0, 0, 0, 1]]), + np.array([[0.346, -0.25, 0.904, -0.117] + [0.904, 0.346, -0.25, 0.473] + [-0.25, 0.904, 0.346, 3.274] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = [[None]] * N + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[i] \ + = np.dot(Xstart, MatrixExp6(MatrixLog6(np.dot(TransInv(Xstart), \ + Xend)) * s)) + return traj + +def CartesianTrajectory(Xstart, Xend, Tf, N, method): + """Computes a trajectory as a list of N SE(3) matrices corresponding to + the origin of the end-effector frame following a straight line + + :param Xstart: The initial end-effector configuration + :param Xend: The final end-effector configuration + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1). The first in the list is Xstart + and the Nth is Xend + This function is similar to ScrewTrajectory, except the origin of the + end-effector frame follows a straight line, decoupled from the rotational + motion. + + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 5 + Output: + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[ 0.937, -0.214, 0.277, 0.811] + [ 0.277, 0.937, -0.214, 0] + [-0.214, 0.277, 0.937, 1.651] + [ 0, 0, 0, 1]]), + np.array([[ 0.277, -0.214, 0.937, 0.289] + [ 0.937, 0.277, -0.214, 0] + [-0.214, 0.937, 0.277, 3.449] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = [[None]] * N + Rstart, pstart = TransToRp(Xstart) + Rend, pend = TransToRp(Xend) + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[i] \ + = np.r_[np.c_[np.dot(Rstart, \ + MatrixExp3(MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \ + s * np.array(pend) + (1 - s) * np.array(pstart)], \ + [[0, 0, 0, 1]]] + return traj + +''' +*** CHAPTER 11: ROBOT CONTROL *** +''' + +def ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, \ + thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd): + """Computes the joint control torques at a particular time instant + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param eint: n-vector of the time-integral of joint errors + :param g: Gravity vector g + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param thetalistd: n-vector of reference joint variables + :param dthetalistd: n-vector of reference joint velocities + :param ddthetalistd: n-vector of reference joint accelerations + :param Kp: The feedback proportional gain (identical for each joint) + :param Ki: The feedback integral gain (identical for each joint) + :param Kd: The feedback derivative gain (identical for each joint) + :return: The vector of joint forces/torques computed by the feedback + linearizing controller at the current instant + + Example Input: + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + eint = np.array([0.2, 0.2, 0.2]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + thetalistd = np.array([1.0, 1.0, 1.0]) + dthetalistd = np.array([2, 1.2, 2]) + ddthetalistd = np.array([0.1, 0.1, 0.1]) + Kp = 1.3 + Ki = 1.2 + Kd = 1.1 + Output: + np.array([133.00525246, -29.94223324, -3.03276856]) + """ + e = np.subtract(thetalistd, thetalist) + return np.dot(MassMatrix(thetalist, Mlist, Glist, Slist), \ + Kp * e + Ki * (np.array(eint) + e) \ + + Kd * np.subtract(dthetalistd, dthetalist)) \ + + InverseDynamics(thetalist, dthetalist, ddthetalistd, g, \ + [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist) + +def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \ + Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, \ + Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes): + """Simulates the computed torque controller over a given desired + trajectory + + :param thetalist: n-vector of initial joint variables + :param dthetalist: n-vector of initial joint velocities + :param g: Actual gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: Actual list of link frames i relative to i-1 at the home + position + :param Glist: Actual spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param thetamatd: An Nxn matrix of desired joint variables from the + reference trajectory + :param dthetamatd: An Nxn matrix of desired joint velocities + :param ddthetamatd: An Nxn matrix of desired joint accelerations + :param gtilde: The gravity vector based on the model of the actual robot + (actual values given above) + :param Mtildelist: The link frame locations based on the model of the + actual robot (actual values given above) + :param Gtildelist: The link spatial inertias based on the model of the + actual robot (actual values given above) + :param Kp: The feedback proportional gain (identical for each joint) + :param Ki: The feedback integral gain (identical for each joint) + :param Kd: The feedback derivative gain (identical for each joint) + :param dt: The timestep between points on the reference trajectory + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an + integer value greater than or equal to 1 + :return taumat: An Nxn matrix of the controllers commanded joint forces/ + torques, where each row of n forces/torques corresponds + to a single time instant + :return thetamat: An Nxn matrix of actual joint angles + The end of this function plots all the actual and desired joint angles + using matplotlib and random libraries. + + Example Input: + from __future__ import print_function + import numpy as np + from modern_robotics import JointTrajectory + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.01 + # Create a trajectory to follow + thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi]) + Tf = 1 + N = int(1.0 * Tf / dt) + method = 5 + traj = mr.JointTrajectory(thetalist, thetaend, Tf, N, method) + thetamatd = np.array(traj).copy() + dthetamatd = np.zeros((N, 3)) + ddthetamatd = np.zeros((N, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamatd[i + 1, :] \ + = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt + ddthetamatd[i + 1, :] \ + = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt + # Possibly wrong robot description (Example with 3 links) + gtilde = np.array([0.8, 0.2, -8.8]) + Mhat01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.1], + [0, 0, 0, 1]]) + Mhat12 = np.array([[ 0, 0, 1, 0.3], + [ 0, 1, 0, 0.2], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + Mhat23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.2], + [0, 0, 1, 0.4], + [0, 0, 0, 1]]) + Mhat34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.2], + [0, 0, 0, 1]]) + Ghat1 = np.diag([0.1, 0.1, 0.1, 4, 4, 4]) + Ghat2 = np.diag([0.3, 0.3, 0.1, 9, 9, 9]) + Ghat3 = np.diag([0.1, 0.1, 0.1, 3, 3, 3]) + Gtildelist = np.array([Ghat1, Ghat2, Ghat3]) + Mtildelist = np.array([Mhat01, Mhat12, Mhat23, Mhat34]) + Ftipmat = np.ones((np.array(traj).shape[0], 6)) + Kp = 20 + Ki = 10 + Kd = 18 + intRes = 8 + taumat,thetamat \ + = mr.SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, \ + Glist, Slist, thetamatd, dthetamatd, \ + ddthetamatd, gtilde, Mtildelist, Gtildelist, \ + Kp, Ki, Kd, dt, intRes) + """ + Ftipmat = np.array(Ftipmat).T + thetamatd = np.array(thetamatd).T + dthetamatd = np.array(dthetamatd).T + ddthetamatd = np.array(ddthetamatd).T + m,n = np.array(thetamatd).shape + thetacurrent = np.array(thetalist).copy() + dthetacurrent = np.array(dthetalist).copy() + eint = np.zeros((m,1)).reshape(m,) + taumat = np.zeros(np.array(thetamatd).shape) + thetamat = np.zeros(np.array(thetamatd).shape) + for i in range(n): + taulist \ + = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, \ + Mtildelist, Gtildelist, Slist, thetamatd[:, i], \ + dthetamatd[:, i], ddthetamatd[:, i], Kp, Ki, Kd) + for j in range(intRes): + ddthetalist \ + = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, \ + Ftipmat[:, i], Mlist, Glist, Slist) + thetacurrent, dthetacurrent \ + = EulerStep(thetacurrent, dthetacurrent, ddthetalist, \ + 1.0 * dt / intRes) + taumat[:, i] = taulist + thetamat[:, i] = thetacurrent + eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent)) + # Output using matplotlib to plot + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + links = np.array(thetamat).shape[0] + N = np.array(thetamat).shape[1] + Tf = N * dt + timestamp = np.linspace(0, Tf, N) + for i in range(links): + col = [np.random.uniform(0, 1), np.random.uniform(0, 1), + np.random.uniform(0, 1)] + plt.plot(timestamp, thetamat[i, :], "-", color=col, \ + label = ("ActualTheta" + str(i + 1))) + plt.plot(timestamp, thetamatd[i, :], ".", color=col, \ + label = ("DesiredTheta" + str(i + 1))) + plt.legend(loc = 'upper left') + plt.xlabel("Time") + plt.ylabel("Joint Angles") + plt.title("Plot of Actual and Desired Joint Angles") + plt.show() + taumat = np.array(taumat).T + thetamat = np.array(thetamat).T + return (taumat, thetamat) diff --git a/packages/Python/setup.py b/packages/Python/setup.py new file mode 100644 index 0000000..5ba8aef --- /dev/null +++ b/packages/Python/setup.py @@ -0,0 +1,44 @@ +from setuptools import setup + +exec(open('modern_robotics/__version__.py').read()) + +long_description = """ +# Modern Robotics: Mechanics, Planning, and Control Code Library + +This package contains the Python code accompanying [_Modern Robotics: +Mechanics, Planning, and Control_](http://modernrobotics.org) (Kevin Lynch +and Frank Park, Cambridge University Press 2017). + +The primary purpose of the provided software is to be easy to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness. + +For more information, including a user manual, see the [project's GitHub page](https://github.com/NxRLab/ModernRobotics). +""" + +setup( + name = "modern_robotics", + version = __version__, + author = "Huan Weng, Mikhail Todes, Jarvis Schultz, Bill Hunt", + author_email = "huanweng@u.northwestern.edu", + description = ("Modern Robotics: Mechanics, Planning, and Control: Code Library"), + license = "MIT", + long_description = long_description, + long_description_content_type='text/markdown', + keywords = "kinematics robotics dynamics", + url = "http://modernrobotics.org/", + packages=['modern_robotics'], + classifiers=[ + "Development Status :: 3 - Alpha", + "Intended Audience :: Education", + "Intended Audience :: Science/Research", + "License :: OSI Approved :: MIT License", + "Natural Language :: English", + "Programming Language :: Python :: 2", + "Programming Language :: Python :: 3", + "Topic :: Education", + "Topic :: Scientific/Engineering", + ], + install_requires=[ + 'numpy', + ], + platforms='Linux, Mac, Windows', +)