Merge branch 'master' into 'main'
First commit after reinitialization See merge request robotics/modern-robotics!1
This commit is contained in:
commit
8967270c39
|
|
@ -0,0 +1,4 @@
|
|||
*.pyc
|
||||
*build/
|
||||
*dist/
|
||||
*.egg-info*
|
||||
|
|
@ -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.
|
||||
105
README.md
105
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.
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
|
||||
```
|
||||
<<ModernRobotics`
|
||||
```
|
||||
|
||||
This process is required for any notebook using this package.
|
||||
|
||||
## Using the Package ##
|
||||
|
||||
After loading 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
|
||||
|
||||
```
|
||||
?RotInv
|
||||
```
|
||||
|
||||
As mentioned in the function usage example, you can try using this function
|
||||
by running
|
||||
|
||||
```
|
||||
invR = RotInv[{{0,0,1},{1,0,0},{0,1,0}}]
|
||||
```
|
||||
|
||||
You should get the the same output as shown in the function usage example.
|
||||
|
||||
## Using the Package Locally ##
|
||||
|
||||
It is possible to use the package locally without installation. Download and
|
||||
place the package anywhere on your filesystem and then run
|
||||
|
||||
```
|
||||
SetDirectory["<PATH-TO-DIRECTORY-CONTAINING-ModernRobotics.m>"]
|
||||
```
|
||||
|
||||
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/
|
||||
```
|
||||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
from .__version__ import __version__
|
||||
|
||||
from .core import *
|
||||
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
|
||||
__version__ = '1.1.0'
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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',
|
||||
)
|
||||
Loading…
Reference in New Issue