UM EECS 398-001

Introduction to Autonomous Robotics


Robot Kinematics and Dynamics

Fall 2017


The AutoRob course provide an introduction to core topics in the modeling and control of autonomous robots. AutoRob has two sections: an undergraduate section offered as Introduction to Autonomous Robotics (EECS 398) and a graduate section offered as Robot Kinematics and Dynamics (ME/EECS 567) with expanded advanced material. The AutoRob course can be thought of as the foundation to build “brains for robots”. That is, given a robot as a machine with sensing, actuation, and computation, how do we build computational models, algorithms, and program that allow the robot to function autonomously? Such computation involves functions for robots to perceive the world, make decisions towards achieving a given objective, and transforming decided actions into motor commands. These functions are essential for modern robotics, especially for mobile manipulators such as the pictured Fetch and PR2 robots.

The AutoRob course focuses on the issues of modeling and control for autonomous robots with an emphasis on manipulation and mobility. Successful completion of AutoRob will result in code modules for "mobile pick-and-place". That is, given a robot and perception of the robot's environment, resulting code modules can enable the robot to pick up an object at an arbitrary location and place the object in a new location.

AutoRob projects ground course concepts through implementation in JavaScript/HTML5 supported by the KinEval code stencil (snapshot below from Mozilla Firefox). These projects will cover graph search path planning (A* algorithm), basic physical simulation (Lagrangian dynamics, numerical integrators), proportional-integral-derivative (PID) control, forward kinematics (3D geometric matrix transforms, matrix stack composition of transforms, axis-angle rotation by quaternions), inverse kinematics, (gradient descent optimization, Jacobians for velocity kinematics), and motion planning (simple collision detection, sample-based motion planning). Additional topics that could be covered include potential field navigation, Bayesian filtering, Cyclic Coordinate Descent, Monte Carlo localization, and Newton-Euler dynamics.

AutoRob projects will roughly follow conventions and structures from the Robot Operating System (ROS) and Robot Web Tools (RWT) software frameworks, as currently used on the Fetch and PR2 robots. These conventions include the URDF kinematic modeling format, ROS topic structure, and JSON-based messaging. Kineval uses threejs for in-browser 3D rendering and Numeric Javascript for select matrix routines. Auxiliary code examples and stencils will often use the jsfiddle development environment.

Related Courses

AutoRob is a computing-friendly pathway into robotics, but does not cover the whole of robotics. The scope of AutoRob is robot modeling and control, which is well-suited as preparation for a Major Design Experience in EECS 467 (Autonomous Robotics Laboratory). AutoRob provides a computation-focused version of ME 567 (Robot Kinematics and Dynamics). The common ME 567 is a more in-depth mathematical analysis of dynamics and control with extensive use of Denavit-Hartenberg parameters for kinematics. AutoRob spends more coverage on path and motion planning with use of quaternions and matrix stacks for kinematics.

AutoRob is also a complement to courses covering perception (EECS 568 Mobile Robotics, EECS 442 Computer Vision), robot building (EECS 498 Hands-on Robotics, ME 552 Mechatronics), robot simulation (ME 543 Analytical and Computational Dynamics), controls systems (EECS 460 Control Systems Analysis and Design, EECS 461 Embedded Control Systems), and artificial intelligence (EECS 492 Introduction to Artificial Intelligence), as well as general graduate courses in robotics (ROB 501 Math for Robotics, ROB 550 Robotic Systems Laboratory).

Commitment to equal opportunity

As indicated in the General Standards of Conduct for Engineering Students, this course is committed to a policy of equal opportunity for all persons and do not discriminate on the basis of race, color, national origin, age, marital status, sex, sexual orientation, gender identity, gender expression, disability, religion, height, weight, or veteran status. Please feel free to contact the course staff with any problem, concern, or suggestion. We ask that all students treat each other with respect.

Accommodations for Students with Disabilities

If you believe an accommodation is needed for a disability, please let the course instructor know at your earliest convenience. Some aspects of this course, the assignments, the in-class activities, and the way the course is usually taught may be modified to facilitate your participation and progress. As soon as you make us aware of your needs, the course staff can work with the Services for Students with Disabilities (SSD, 734-763-3000) office to help us determine appropriate academic accommodations. SSD typically recommends accommodations through a Verified Individualized Services and Accommodations (VISA) form. Any information you provide is private and confidential and will be treated as such. For special accommodations for any academic evaluation (exam, quiz, project), the course staff will need to receive the necessary paperwork issued by the SSD office by September 29, 2017.

Student mental health and well being

The University of Michigan is committed to advancing the mental health and wellbeing of its students. If you or someone you know is feeling overwhelmed, depressed, and/or in need of support, services are available. For help, please contact Counseling and Psychological Services (CAPS) by phone at 734-764-8312, during and after hours, on weekends and holidays, or through its counselors physically located in schools on both North and Central Campus. You may also consult University Health Service (UHS, 734-764-8320) as well as its services for alcohol or drug concerns. There is also a more comprehensive listing of mental health resources available on and off campus.

Course Staff


Chad Jenkins
ocj addrsign umich
Office: Beyster 3644
Office Hours: Monday 3-5pm, Tuesday 2-4pm


Zhen Zeng
zengzhen addrsign umich
Office Hours (Beyster 1637 A): Wednesday 3-5pm, Thursday 3-5pm

Meeting time/place

Monday, Wednesday 1:30-3:00
DOW 2150

Discussion channel

AutoRob #general channel

Note: General course discussion and administrivia will be addressed in the #general channel. Each assignment will have its own discussion channel. Random items of general interest to the course and larger field of robotics will appear in the #random channel. Actively engaging in course discussions is a great way to become a better roboticist.


This course has recommended prerequisites for "Linear Algebra" and "Data Structures and Algorithms", or permission from the instructor.

Programming proficiency: EECS 281 or proficiency in data structures and algorithms should provide an adequate programming background for the projects in this course. Interested students should consult with the course instructor if they have not taken EECS 281 or its equivalent, but have some other strong programming experience.

Mathematical proficiency: Math 214, 217, 417, 419 or proficiency in linear algebra should provide an adequate mathematical background for the projects in this course. Interested students should consult with the course instructor if they have not taken one of the listed courses or their equivalent, but have some other strong background with linear algebra.

Recommended optional proficiency: Differential equations, Computer graphics, Computer vision, Artificial Intelligence

The instructor will do their best to cover the necessary prerequisite material, but no guarantees. Linear algebra will be used extensively in relation to 3D geometric transforms and systems of linear equations. Computer graphics is helpful for under-the-hood understanding of threejs. Computer vision and AI share common concepts with this course. Differential equations are used to cover modeling of motion dynamics and inverse kinematics, but not explicitly required.


The AutoRob course is compatible with both the Spong et al. and Corke textbooks (listed below), although only one of these books is needed. Depending on individual styles of learning, one textbook may be preferrable over the other. Spong et al. is the listed required textbook for AutoRob (as well as ME 567) and is supplemented with additional handouts. The Corke textbook provides broader coverage with an emphasis on intuitive explanation.

Robot Modeling and Control
Mark W. Spong, Seth Hutchinson, and M. Vidyasagar
Wiley, 2005
Available at Amazon

Alternate textbook

Robotics, Vision and Control: Fundamental Algorithms in MATLAB
Peter Corke
Springer, 2011

Optional texts

JavaScript: The Good Parts
Douglas Crockford
O'Reilly Media / Yahoo Press, 2008

Principles of Robot Motion
Howie Choset, Kevin M. Lynch, Seth Hutchinson, George A. Kantor, Wolfram Burgard, Lydia E. Kavraki, and Sebastian Thrun
MIT Press, 2005

Projects and Grading

The AutoRob course will assign 7 projects (6 programming, 1 oral) and 5 quizzes. Each project has been decomposed into a collection of features, each of which is worth a specified number of points. AutoRob project features are graded as “checked” (completed) or “due” (incomplete). Prior to being assigned, upcoming projects will have the status of "pending." In terms of workload, each project is expected to take approximately 4 hours of work on average (as a rough estimate). Each quiz will consist of 4 short questions that will be within the scope of previously graded projects. In other words, each quiz question should be readily answerable given knowledge from correctly completing projects on time.

Individual final grades are assigned based on the sum of points earned from coursework (detailed in subsections below). The timing and due dates for course projects and quizzes will be announced on an ongoing basis. All project work must be checked by the end of classes.

EECS 398-001: Introduction to Autonomous Robotics

In the undergraduate section, each fully completed project is weighted as 12 points and each correctly answered quiz question is weighted as 1 point. Based on this sum of points from coursework, an overall grade for the course is earned as follows: An "A" grade in the course is earned if graded coursework sums to 93 points or above; A "B" grade in the course is earned if graded coursework sums to 83 points or above; a "C" grade in the course is earned if graded coursework sums to 73 points or above. The instructor reserves the option to assign appropriate course grades with plus or minus modifiers.

ME/EECS 567: Robot Kinematics and Dynamics

In the graduate section, each fully completed project is weighted as 18 points, each correctly answered quiz question is weighted as 1 point. Students in the EECS 598-009 have the opportunity to earn 4 additional points through an advanced extension of a course project. Examples of advanced extensions include implementation of RK4 numerical integration for a double pendulum, inverse kinematics by Cyclic Coordinate Descent, one additional motion planning algorithm, point cloud segmentation, and a review of a current research publication in robotics. Based on this sum of points from coursework, an overall grade for the course is earned as follows: An "A" grade in the course is earned if graded coursework sums to 135 points or above; A "B" grade in the course is earned if graded coursework sums to 120 points or above; a "C" grade in the course is earned if graded coursework sums to 105 points or above. The instructor reserves the option to assign appropriate course grades with plus or minus modifiers.

Project Rubric (tenative and subject to change)

The following project features are planned for AutoRob this semester. Students enrolled in ME/EECS 567 will complete all features. Students in the undergraduate section are not expected to implement features for the graduate section.

Assignment 1: 2D Path Planning
4All Heap implementation
8All A-star search
2567 BFS
2567 DFS
2567 Greedy best-first
Assignment 2: Pendularm
4All Euler integrator
4All Velocity Verlet integrator
4All PID control
1567 Verlet integrator
2567 RK4 integrator
3567 Double pendulum
Assignment 3: Forward Kinematics
2All Core matrix routines
8All FK transforms
2All Joint selection/rendering
2567 Base offset transform
4567 Fetch DH table
Assignment 4: Dance Controller
6All Quaternion joint rotation
2All Interactive base control
2All Pose setpoint controller
2All Dance FSM
2567 Joint limits
2567 Prismatic joints
2567 Fetch rosbridge interface
Assignment 5: Inverse Kinematics
6All Manipulator Jacobian
3All Gradient descent with Jacobian transpose
3All Jacobian pseudoinverse
6567 Euler angle conversion
Assignment 6: Motion Planning
4All Collision detection
2All 2D RRT-Connect
6All Configuration space RRT-Connect
6567 RRT-Star

Project Submission and Regrading

Git repositories will be used for project implementation, version control, and submission. The implementation of an individual project is submitted through an update to the master branch of your designated repository. Updates to the master branch must be committed and pushed prior to the due date for each assignment for any consideration of full credit. Your implementation will be checked out and executed by the course staff. Through your repository, you will be notified by the course staff whether your implementation of assignment features is sufficient to receive credit.

Late Policy

Do not submit assignments late. The course staff reserves the right to not grade late submissions. The course instructor reserves the right to decline to grade late submissions and adjust partial credit on regraded assignments. If granted by the course instructor, late submissions can be graded for partial credit, with a guideline as follows. Submissions pushed within two weeks past the project deadline will be graded for 80% credit. Submissions pushed within four weeks of the project deadline will be graded for 60% credit. Submissions pushed at any time before the semester project submission deadline (December 15, 2017) will be considered for 50% credit. The course instructor reserves the right to decline to grade late submissions and adjust partial credit on regraded assignments.

Regrading Policy

The regrading policy allows for submission and regrading of projects up through the final grading of projects, which will be December 15 for the Fall 2017 Semester. This regrading policy will grant full credit for project submissions pushed to your repository before the corresponding project deadline. If a feature of graded project is returned as not completed (or "DUE"), your code can updated for consideration at 80% credit. This code update must be pushed to your repository within two weeks from when the originally graded project was returned. Regrades of projects updated beyond this two week window can receive at most 60% credit.

Final Grading

All grading will be finalized on December 15, 2017. Regrading of specific assignments can be done upon request during office hours. No regrading will be done after grades are finalized.


You are expected to provide a private git repository for your work in this course with the course instructor added as a read/write collaborator. If needed, the course staff can assist in the setup of an online git repository through providers such as github or bitbucket, or internally through the EECS gitlab server.

There are many different tutorials for learning how to use git repositories. The AutoRob course site has its own basic quick start tutorial. The EECS gitlab server has a basic quick start tutorial. The Pro Git book provides an in-depth introduction to git and version control. As different people often learn through different styles, the Git Magic tutorial has also proved quite useful when a different perspective is needed. git: the simple guide has often been a great and accessible quick start resource.

We expect students to use these repositories for collaborative development as well as project submission. It is the responsibility of each student group to ensure their repository adheres to the Collaboration Policy and submission standards for each assignment. Submission standards and examples will be described for each assignment as needed.

Collaboration Policy

This policy covers all course material and assignments unless otherwise stated. Course material, concepts, and documentation may be discussed with anyone. Assignments may be discussed with the other students at the conceptual level. Discussions may make use of a whiteboard or paper. Discussions with others (or people outside of your assigned group) cannot include writing or debugging code on a computer or collaborative analysis of source code that is not your own. You may take notes away from these discussions, provided these notes do not include any source code.

The code for your implementation may not be shown to anyone outside of your group, including granting access to repositories or careless lack of protection. You do not need to hide the screen from anyone, but you should not attempt to show anyone your code. When you are done using any robot device such that another group may use it, you must remove all code you have put onto the device. You may not share your code with others outside of your group. At any time, you may show others the implemented program running on a device or simulator, but you may not discuss specific debugging details about your code while doing so.

This policy applies not only applies to collaboration during the current semester, but also any past or future instantiations of this course. Although course concepts are intended for general use, your implementation for this course must remain private after the completion of the course. It is expressly prohibited to share any code previously written and graded for this course with students currently enrolled in this course. Similarly, it is expressly prohibited for any students currently enrolled in this course to refer to any code previously written and graded for this course.

To acknowledge compliance with this collaboration policy, include a file named "honor.txt" in the main directory of your repository with the following text: "I have neither given nor received unauthorized aid on this course project implementation, nor have I concealed any violations of the Honor Code." This text will be considered updated with the current date and time of each commit to your repository. Repository commits that do not include this honor file will not be graded as the discretion of the course advisor.

Should you fail to abide by this policy, you will receive no credit for this course. The University of Michigan reserves the right to pursue any means necessary to ensure compliance. This includes, but is not limited to prosecution through The College of Engineering’s Honor Council, which can result in your suspension or expulsion from the University of Michigan. Please refer to the Engineering Honor Council for additional information.

Course Schedule (tentative and subject to change)

Note: Assignment descriptions will have updated assignment due dates. Assignment due dates listed in the schedule are merely a guide.
Sep 6 Initialization: Course overview, Robotics roadmap, Path planning quick start
Spong Ch.1
Corke Ch.1
Out: Path Planning
What is a robot?: Brief history and definitions for robotics
Week 2
Sep 11 Path Planning: DFS, BFS, A-star, Greedy best first
JavaScript and git tutorial: Heap sort example
Crockford, hello.html, hello_anim, hello_anim_text
Sep 13 Pendulum Simulation and Numerical Integration: Lagrangian equation(s) of motion, Initial value problem, Explicit integrators: Euler, Verlet, and Runge-Kutta 4
Handout: 1, 2, 3;
Witkin&Baraff 1998: Dynamics, Integrators
Week 3
Sep 18 Motion Control: Cartesian vs. generalized coordinates, open-loop vs. closed-loop control, PID control; Rigid body dynamics
Spong 6.3
Due: Path Planning
Out: Pendularm
Sep 20 Linear Algebra Refresher: Systems of linear equations, vector spaces and operations, matrix operations, least squares approximations
Spong A-B
Corke D
Week 4
Sep 25 No class - IROS 2017
Sep 27 Quiz 1
Week 5
Oct 2 Forward Kinematics: Kinematic chains, URDF, homogeneous transforms, matrix stack traversal, D-H convention
Spong 3.1-2
Corke 7.1-2
Oct 4 Axis-angle Rotation and Quaternions: Motors, Euler angles, gimbal lock, Rodrigues rotation, rotation in complex spaces Handout 1, 2
Corke 2.2-3
Due: Pendularm
Out: Forward Kinematics
Week 6
Oct 9 Reactive Controllers: Reactive and Deliberative Decision Making, Finite State Machines, Subsumption Architecture
Brooks 1986
Oct 11 Quiz 2 IK robot game
Week 7
Oct 16 No class - Fall Study Break
Oct 18 Inverse Kinematics 1 - Closed-form: Joint vs. Endeffector control, Planar 2-link arm, Closed form solutions, Cyclic Coordinate Descent
Spong 3.3
Corke 7.3
Due: Forward Kinematics
Out: Dance Contest
Week 8
Oct 23 Inverse Kinematics 2 - Iterative: Gradient descent, Manipulator Jacobian, Jacobian transpose, pseudoinverse Spong Ch. 4, Wang, Chen 1991, Buss 2009
Corke Ch. 8
Oct 25 Robot Middleware: Hardware Abstraction, ROS, LCM, Publish-subscribe messaging, rosbridge, Client-server messaging
Quigley 2009, Huang 2010, Toris 2015
Week 9
Oct 30 Bug Algorithms: Reaction vs. Deliberation revisted, Bug[0-2], Tangent Bug Corke Ch. 5
Nov 1 Configuration Spaces: Curse of dimensionality, Configuration space vs. Workspace, Minkowski planning, Costmaps, Holonomicity Due: Dance Contest
Out: Inverse Kinematics
Week 10
Nov 6 Quiz 3
Nov 8 Sample-based Planning: Probabilistic roadmaps, RRT-based motion planning Kavraki et al. 1996, Kuffner, LaValle 1999
Potential fields: Gradient descent revisited, local search, downhill simplex, Wavefront planning Khatib 1986, Jarvis 1993, Zelinsky 1992
Week 12
Nov 13 ROS/catkin tutorial session Example tutorial result
Nov 15 Collision Detection: 3D Triangle-Triangle Testing, Oriented Bounding Boxes, Axis-Aligned Bounding Boxes, Separating Axis Theorem Gottschalk et al. 1996, Moller 1997 Due: Inverse Kinematics
Out: Motion Planning
Week 13
Nov 20 Quiz 4
Nov 22 No class - Thanksgiving Recess
Week 14
Nov 27 3D Point Cloud Segmentation: Point cloud segmentation, Principal Components Analysis, Connected components PCA
Rusu 2008 (Sec 4.2)
Nov 29 Localization and Mapping Overview: Bayes rule, Bayesian filtering, Monte Carlo Localization, Factor Graphs, SGD-SLAM, Scene estimation Dellaert 1999
Olson 2006
Sui 2015
Week 15
Dec 4 Extended office hours
Dec 6 Quiz 5 Due: Motion Planning
Week 16
Dec 11 Due: Best Use of Robotics
Dec 15 Grading finalized

Slides from this course borrow from and are indebted to many sources from around the web. These sources include a number of excellent robotics courses at various universities.

Assignment 1: Path Planning

Due 11:59pm, Thursday, September 21, 2017

The objective of the first assignment is to implement a collision-free path planner in JavaScript/HTML5. Path planning is used to allow robots to autonomously navigate in environments from previously constructed maps, which can be estimated through simultaneous localization and mapping. For this assignment, you will implement the A-star graph search algorithm for generating the shortest path from a start to a goal location in an arbitrary 2D world. This A-star implementation will consider locations in a uniformly space grid. You will implement a heap data structure as a priority queue for visiting these locations.

If properly implemented, the A-star algorithm should produce the following path (or path of similar length) using the provided code stencil:

Cloning the Stencil Repository

The first step for completing this project (and all projects for AutoRob) is to clone the KinEval stencil repository Fall 2016. The appended git quick start below is provided those unfamiliar with git to perform this clone operation, as well as commiting and pushing updates for project submission. IMPORTANT: the stencil repository should be cloned and not forked.

Throughout the KinEval code stencil, there are markers with the string "STENCIL" for code that needs to be completed for course projects.

Heap Sort Tutorial

For those new to JavaScript/HTML5, the recommended starting point is to complete the heap sort implementation in the "tutorial_heapsort" subdirectory of the stencil repository. In this directory, a code stencil in JavaScript/HTML5 is provided in two files: "heapsort.html" and "heap.js". Comments are provided throughout these files to describe the structure of JavaScript/HTML5 and its programmatic features. In addition, there are other tutorial-by-example files in the "tutorial_js" directory. Any of these files can be run by simply opening them in a web browser.

Opening "heapsort.html" will show the result of running the incomplete heap sort implementation provided by the code stencil:

To complete the heap sort implementation, complete the heap implementation in heap.js at the locations marked "STENCIL". In addition, the inclusion of heap.js in the execution of the heap sort will require modification of heapsort.html.

A successful heap sort implementation will show the following result for a randomly generated set of numbers:

Graph Search Stencil

For the path planning implementation, a JavaScript/HTML5 code stencil has been provided in the file "search_canvas.html" within the "project_pathplan" subdirectory. Opening this file in a browser should display an empty 2D world displayed in an HTML5 canvas element.

There are five planning scenes that have been provided within this code stencil: "empty", "misc", "narrow1", "narrow2", and "three_sections". The choice of planning_scene can be specified from the URL given to the browser, described in the usage in the file. For example, the URL "search_canvas.html?planning_scene=narrow2" will bring up the "narrow2" planning world. Other execution parameters, such as start and goal location, can also be specified through the document URL.

This code stencil is implemented to perform graph search iterations interactively in the browser. The core of the search implementation is performed by the function iterateGraphSearch(). This function performs a graph search iteration for a single location in the A-star execution. The browser implementation cannot use a while loop over search iterations, as in the common A-star implementation. Such a while loop would keep control within the search function, and cause the browser to become non-responsive. Instead, the iterateGraphSearch() gives control back to the main animate() function, which is responsible for updating the display and user interaction.

Within the code stencil, you will complete the functions initSearchGraph() and iterateGraphSearch() as well as add functions for heap operations. Locations in "search_canvas.html" where code should be added are labeled with the "STENCIL" string. In initSearchGraph() creates a 2D array over locations to be searched. Each element of this array contains various information computed by the search algorithm for a particular location. iterateGraphSearch() should use three of the provided functions to perform a search iteration. testCollision() returns a boolean of whether a given 2D location, as a two-element vector, is in collision with the planning scene. draw_2D_configuration() draws a square at a given location in the planning world to indicate that location has been explored. Once the search is complete, drawHighlightedPathGraph() will render the path produced by the search algorithm between a given location and the start location. The global variable search_iterate should be set to false to end animation loop.

Graduate Section Requirement

In addition to the A-star algorithm, students in the graduate section of AutoRob must additionally implement path planning by Depth-first search, Breadth-first search, and Greedy best-first search. An additional report, as file "report.html", is required that: 1) shows results from executing every search algorithm with every planning world for various start and goal configurations and 2) synthesizes these results into coherent findings about these experiments.

Advanced Extensions

Advanced extensions can be submitted anytime before the final grading is complete. Concepts for several of these extensions will not be covered until later in the semester.

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by implementing the Bug0, Bug1, Bug2, and TangentBug navigation algorithms. This bug algorithm implementation should be contained within the file "search_canvas.html" under the "project_pathplan" directory.

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by implementing navigation by potential fields and navigation using the Wavefront algorithm. This potential-based navigation implementation should be contained within the file "search_canvas.html" under the "project_pathplan" directory.

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by implementing a navigation algorithm using a probabilistic roadmap. This roadmap algorithm implementation should be contained within the file "search_canvas.html" under the "project_pathplan" directory.

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by implementing costmap functionality using morphological operators. Based on the computed costmap, the navigation routine would provide path cost in addition path length for a successful search.

Project Submission

For turning in your assignment, ensure your completed project code has been committed and pushed to the master branch of your repository.

To ensure proper submission of your assignments, please do the following:

You are encouraged to update your repository often using branching. The master branch should be used for the working (or stable) version of your code. For development, you can create an Assignment-1 branch can be created for your work on this project as it in development and experimentation. Similarly, an Assignment-2 branch can be created for the next project as it develops. Once you complete an assignment, the code from this branch can then be merged into the master branch for grading. This configuration allows your work to be continually updated and build upon such that versions are tracked and interruptions due to grading are minimized.

Assignment 2: Pendularm

Due 11:59pm, Friday, October 6, 2017

As an introduction to physical dynamics and control, your task is to implement a physical simulator and servo controller for a frictionless simple pendulum with a rigid massless rod, and then control this system as a 1 DOF robot with a single motor, a visualization of which is shown below.

The code stencil for the Pendularm assignment is available within the "pendularm" subdirectory of KinEval in the file pendularm1.html.

For physical simulation, you will implement several numerical integrators for a pendulum with parameters specified in the code stencil. The numerical integrator will advance the state (angle and velocity) of the pendulum in time given the current acceleration (generated from the pendulum equation of motion). If implemented successfully, this ideal pendulum should oscillate about the vertical (where the angle is zero) and with an amplitude that preserves the initial height of the pendulum bob.

Students enrolled in the undergraduate section will implement numerical integrators for:

For motion control, students in both undergraduate and sections will implement a proportional-integral-derivative controller to control the system's motor to a desired angle. This PID controller should output control forces integrated into the system's dynamics. You will need to tune the gains of the PID controller for stable and timely motion to the desired angle for pendulum with parameters: length=2.0, mass=2.0, gravity=9.81.

For user input, you should be able to:

Graduate Section Requirement

Students enrolled in the graduate section will implement numerical integrators for:

to simulate and control both a single pendulum (in "pendulum1.html") a double pendulum (by creating "pendulum2.html"). A code stencil update will be provided for pendularm2. A working visualization for the double pendularm will look similar to the following:

Advanced Extensions

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by implementing a simulation of a planar cart pole system. This cart pole implementation should be contained within the file "cartpole.html" under the "project_pendularm" directory.

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by implementing a single pendulum simulator in maximal coordinates with a spring constraint enforced by Gauss-Seidel optimization. This maximal coordinate pendulum implementation should be contained within the file "pendularm1maximal.html" under the "project_pendularm" directory. An additional point can be earned by extending this implementation to a cloth simulator in the file "cloth_pointmass.html".

Project Submission

For turning in your assignment, push your updated code to the master branch in your repository.

Assignment 3: Forward Kinematics

Due 11:59pm, Friday, October 20, 2017

In this assignment, you will render the forward kinematics (FK) of a robot, given its kinematic specification (in the "robots" subdirectory). To render the robot properly, you will compute matrix coordinate frame transforms for each link and joint of the robot based on the parameters of its hierarchy configuration. The computation of the matrix transform for each joint and link will allow KinEval's rendering support routines to properly display the full robot. We will assume the joints will remain in their zero position, saving joint motion for the next assignment.

Just Starting Mode

kineval_stencil contains a code template for this assignment as well as all future projects in the course. This KinEval stencil allows for developing the core of a modeling and control computation stack (forward Kinematics, inverse kinematics, and motion planning) in a modular fashion.

If you open "home.html" in this repository, you should see the jittering disconnected pieces of a robot (described in "robots/mr2.js"), similar to the snapshot below. This initial mode is the "starting point" state of the stencil to help build familiarity with JavaScript/HTML5 and KinEval.

Your task is to make these objects in starting point mode responsive to keyboard commands. Specifically, these objects will move upward, stop/start jittering, move closer together, and further apart (although more is encouraged). To do this, you will modify "kineval/kineval_startingpoint.js" at the sections marked with "STENCIL". These sections also include code examples meant to be a quick (and very rough) introduction to JavaScript, assuming programming competency in another language.

Brief KinEval Stencil Overview

Within the KinEval stencil, the functions my_animate() and my_init() in "home.html" are the principal access points into animation system. my_animate() is particularly important as it will direct the invocation of functions we develop throughout the AutoRob course. my_animate() and my_init() are called by the primary process that maintains the animation loop: kineval.animate() and kineval.init() within "kineval/kineval.js". IMPORTANT: "kineval/kineval.js", kineval.animate(), kineval.init(), and any of the given robot descriptions should not be modified.

For starting point mode, my_animate() will call startingPlaceholderAnimate() and startingPlaceholderInit(). startingPlaceholderInit() contains JavaScript tutorial-by-example code that initializes variables for this project. startingPlaceholderAnimate() contains keyboard handlers and code to update the positioning of each body of the robot. By modifying the proper variables at the locations labed "STENCIL", this code will update the transformation matrix for each geometry of the robot (stored in the ".xform" attribute) as a translation in the robot's world. The ".xform" transform for each robot geometry is then used by kineval.robotDraw() to have the browser render the robot parts in the appropriate locations.

Forward Kinematics

Assuming proper completion of Just Starting Mode, you are now ready for implementation of robot forward kinematics. Ensure the following files are included (within script tags) in your "home.html". You will modify these files for implementing FK:

Robot Examples and Initialization

Each file in the "robots" subdirectory contains code to create a robot data object . This data object is initialized the kinematic description of a robot (as well as some meta information and rendering geometries). The kinematic description defines a hierarchical configuration of the robot's links and joints. This description is a subset of the Unified Robot Description Format (URDF) converted into JSON format. The basic features of URDF are described in this tutorial.

IMPORTANT: The given robot description files should NOT be modified. Code that requires modified robot description files will fail tests used for grading. You are welcomed and encouraged to create new robot description files for additional testing.

The selection of file with a robot description can occur directly in the URL for "home.html". As a default, the "home.html" in the KinEval stencil assumes the "mr2" robot description in "robots/robot_mr2.js". Another robot description file can be selected directly in the URL by adding a robot parameter. This parameter is segmented by a question mark and sets the robot file pointer to a given file local location, relative to "home.html". For example, a URL with "home.html?robot=robots/robot_urdf_example.js" will use the URDF example description.

In addition to the given initialization, you should extend the robot object to complete the kinematic hierarchy to specify the parent and children of each link. This modification should be made in the kineval.initRobotJoints() function in "kineval/kineval_robot_init.js". The children array of a link should always be defined, which would result in an empty array for leaf nodes in the kinematic tree.

Note: KinEval refers to links and joints as strings, not pointers, within the robot object. robot.joints (as well as robot.links) is an array of data objects that are indexed by strings. Each of these objects stores relevant fields of information about the joint, such as its transform (".xform"), parent (".parent") and child (".child") in the kinematic hierarchy, local transform information (".origin"), etc. As such, robot.joints['JointX'] refers to an object for a joint. In contrast, robot.joints['JointX'].child refers to a string ('LinkX'), that can then be used to reference a link object (as robot.links['LinkX']). Similarly, robot.links['LinkX'].parent refers to a joint as a string 'JointX' that can then then be used to reference a joint object in the robot.joints array.

Invoking Forward Kinematics

The function kineval.robotForwardKinematics() in "kineval/kineval_forward_kinematics.js" will be the main point of invocation for your FK implementation. This function is responsible for updating matrix transforms for the frame of each link and joint with respect to the global world coordinates. The computed transform for each frame of the robot needs to be stored in the ".xform" field. For a given link named 'LinkX', this xform field can be accessed as robot.links['LinkX'].xform. For a given joint named 'JointX', this xform field can be accessed as robot.joints['JointX'].xform. Once kineval.robotForwardKinematics() completes, the updated transforms for each frame are used by the function kineval.robotDraw() in the support code to render the robot.

A matrix stack recursion can be used to compute these global frames, starting from the base of the robot (specified as a string in robot.base). This recursion should use local translation and rotation parameters of each joint in relation to its parent link in its traversal of the hierarchy. For a given joint 'JointX', these translation and rotation parameters are stored in the robot object as robot.joints['JointX'] and robot.joints['JointX'].origin.rpy, respectively. The current global translation and rotation for the base of the robot (robot.base) in the world coordinate frame is stored in and robot.origin.rpy, respectively.

To run your FK routine, you must toggle out of starting point mode. This toggle can be done interactively within the GUI menu or by setting kineval.params.just_starting to false. The code below in "home.html" controls starting point mode invocation, where single line can be uncommented to use FK mode by default:

// set to starting point mode is true as default 
//   set to false once starting forward kinematics project
//kineval.params.just_starting = true;

if (kineval.params.just_starting == true) {

If implemented properly, the "robots/robot_urdf_example.js" example should produce the following rendering:

The "robots/robot_mr2.js" example should produce the following:

The "robots/robot_crawler.js" example should produce the following:

Interactive Hierarchy Traversal

Additionally, a correct implementation will be able to interactively traverse the kinematic hierarchically by changing the active joint. The active joint has focus for user control, which will be used in the next assignment. For now, we are using the active joint to ensure your kinematic hierarchy is correct. You should be able to move up and down the kinematic hierarchy with the "k" and "j" keys, respectively. You can also move between the children of a link using the "h" and "l" keys.

Orienting Joint Rendering Cylinders

The cylinders used as rendering geometries for joints are not aligned with joint axes by default. The support code in Kineval will properly orient joint rendering cylinders. To use this functionality, simply implement a vector cross product function named vector_cross() in your matrix routines. vector_cross() will be automatically detected and used to properly orient each joint rendering cylinder.

Graduate Section Requirement

Students in the AutoRob Graduate Section must implement the assignment as described above to work with given examples as well as the Fetch robot description. Students in the graduate section must also generate a proper Denavit-Hartenberg table for the kinematics of the Fetch robot.

The file "robots/fetch/fetch.urdf/js" contains the robot data object for the Fetch kinematic description. This JavaScript file converted from the Fetch URDF description for ROS. ROS uses a different default coordinate system than threejs, which needs to be taken into account in the FK computation. Coordinate frames in ROS assumes that the Z, X, and Y axes correspond to the up, forward, and side directions, respectively. In contrast, threejs coordinate frames assume the Y, Z, and X correspond to the up, forward, and side directions. The variable robot.links_geom_imported will be set to true when geometries have been imported from ROS and set to false when geometries are defined completely within the robot description file.

Advanced Extensions

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by creating a new robot description. This robot description should be placed in the "robots" directory with a filename with your username in the format "robot_uid.js".

Of the 4 possible advanced extension points, three additional points for this assignment can be earned by implementing LU decomposition routines for matrix inversion and solving linear systems. These functions should be named "matrix_inverse" and "linear_solved" and placed within the file containing your matrix routines.

Project Submission

For turning in your assignment, push your updated code to the master branch in your repository.

A proper implementation for fetch.urdf.js example should produce the following:

Assignment 4: Robot FSM Dance Contest

Due 11:59pm, Friday, November 3, 2017

For this assignment, you will now enable your robot to execute a dance routine by adding motor rotation to its joints and creating a Finite State Machine (FSM) controller over pose setpoints. Your FK implementation will be extended to consider angular rotation about each joint axis using quaternions for axis-angle rotation. The positioning of each joint with respect to a given pose setpoint will be controlled by an simple P servo implementation (based on the Pendularm assignment). You will implement an FSM controller to update the current pose setpoint based on the robot's current state and predetermined sequence of setpoints. For a single robot, you will choreograph a dance for the robot by creating an FSM with your design of pose setpoints and an execution sequence.

This controller for the "mr2" was a poor attempt at robot Saturday Night Fever (please do better):

This updated dance controller for the Fetch robot is a bit better, but still very far from optimal:

Assuming proper completion of Assignment 4, ensure the following files are included (within script tags) in your "home.html". You will modify these files for implementing axis-angle rotation, the pose setpoint controller, and the FSM controller:

Joint Axis Rotation and Interactive Control

Each joint of the robot needs several additional properties for joint rotation and control. These joint properties for the current angle rotation (".angle"), applied control (".control"), and servo parameters (".servo") have already been created within the function kineval.initRobotJoints(). The joint's angle will be used to calculate a rotation about the joints (normal) axis of rotation vector, specified in the ".axis" field. The 3D rotation due to joint movement should be accounted for in the robot's forward kinematics and implemented as quaternions in "kineval/kineval_quaternion.js".

If joint axis rotation is implemented correctly, you should be able to use the 'u' and 'i' keys to move the currently active joint. These keys respectively decrement and increment the ".control" field of the active joint. Through the function kineval.applyControls(), this control value effectively adds an angular displacement to the joint angle.

Interactive Base Movement Controls

The user interfaces also enables controlling the global position and orientation of the robot base. In addition to joint updates, the system update function kineval.applyControls() also updates the base state (in robot.origin) with respect to its controls (specified in robot.controls). With the support function kineval.handleUserInput(), the 'wasd' keys are purposed to move the robot on the ground plane with 'q' and 'e' keys for lateral base movement. In order for these keys to behave properly, the heading and lateral directions of the robot base are needed such that they respectively express coordinates along local z-axis and x-axis of the base in the global frame. These vectors need to be computed within your FK implementation and stored within two global variables: robot_heading and robot_lateral. Each of these variables should be a homogeneous 3D vector stored as a 2D array.

If robot_heading and robot_lateral are implemented properly, the robot should now be interactively controllable in the ground plane.

Pose Setpoint Controller

Once joint axis rotation is implemented, you will implement proportional setpoint controller for the robot joints in function kineval.robotArmControllerSetpoint() within "kineval/kineval_servo_controller.js". This setpoint controller uses the current angle (".angle"), desired angle, and servo gains (specified in the ".servo" object) of each joint to output a control (".control") for the joint. The desired angle for a joint 'JointX' is stored in kineval.params.setpoint_target['JointX'] as a scalar. All of these joint object properties are initialized in the function kineval.initRobotJoints() in "kineval/kineval_robot_init.js".

For testing, a "clock movement" controller has been provided as the function setpointClockMovement() in "kineval/kineval_setpoint_controller.js". This function can be invoked by holding down the 'c' key or from the UI. This controller goes well with this song.

The robot can servo to the current pose setpoint by holding down the 'o' key or selecting 'persist_pd' from the UI. Pressing the '0' key sets the current setpoint to the zero pose, where all joint angles are zero. Stored in kineval.setpoints, up to 9 other arbitrary pose setpoints can be stored by KinEval for pose control. The current robot pose can be interactively stored by pressing "Shift+number_key" (e.g., "Shift+1"). The current setpoint can be assigned a stored pose by pressing one of the non-zero number keys [1-9]. At any time, the currently stored setpoints can be output to the console as JavaScript code using the JSON.stringify function for the setpoint object, as the statement "JSON.stringify(kineval.setpoints);". This setpoint array can be included in your code as part of your dance controller.

FSM Controller

Once your pose setpoint controller is work, a FSM controller should be implemented in the function kineval.setpointDanceSequence() in "kineval/kineval_setpoint_control.js". The reference implementation uses pose setpoints initialized and stored in kineval.setpoints with a sequence of indices stored in kineval.params.dance_sequence_index and playback index stored in kineval.params.dance_pose_index. If this convention is not used, the following line in "kineval/kineval_userinput.js" will require modification:

if (kineval.params.update_pd_dance)
    textbar.innerHTML += "executing dance routine, pose " + kineval.params.dance_pose_index + " of " + kineval.params.dance_sequence_index.length;

Graduate Section Requirement

Students in the graduate section of AutoRob must implement the assignment as described above for the Fetch robot with two additional requirements: 1) proper enforcement of joint types and limits for Fetch robot description, and 2) integration (via rosbridge) of their code with ROS or a Gazebo simulation of the Fetch.

The Fetch URDF JS file, included in the provided code stencil, contains joints with with various types that correspond to different types of motion:

Joints are considered to be continuous as the default. Joints with undefined motion types must be treated as continuous joints.

Your code can interface with any robot (or simulated robot) running rosbridge/ROS using the function kineval.rosbridge() in "kineval/kineval_rosbridge.js". This code requires that the rosbridge_server package is running in a ROS run-time environment and listening on a websocket port, such as for ws://fetch7:9090. If your FK implementation is working properly, the model of your robot in the browser will update along with the motion of the robot based on the topic subscription and callback. This functionality works seamlessly between real and simulated robots. Although this will not be done for this class, to control the robot arm, a rosbridge publisher must be written to update the ROS topic "/arm_controller/follow_joint_trajectory/goal" with a message of type "control_msgs/FollowJointTrajectoryActionGoal".

Machines running rosbridge, ROS, and Gazebo for the Fetch will be available during special sessions of the class. Students are encouraged to install and run the Fetch simulator on their own machines based on this tutorial.

Advanced Extensions

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by adding the capability of displaying laser scans from a real or simulated Fetch robot.

Of the 4 possible advanced extension points, four additional points for this assignment can be earned by adding the capability of displaying 3D point clouds from a real or simulated Fetch robot and computing surface normals about each point.

Of the 4 possible advanced extension points, four additional points for this assignment can be earned by implementing dynamical simulation through the recursive Newton-Euler algorithm (Spong Ch.7). This dynamical simulation update be implemented as function kineval.updateDynamicsNewtonEuler() in the file "kineval/kineval_controls.js". In "home.html", the call to kineval.updateDynamicsNewtonEuler() should replace the call purely kinematic update in kineval.applyControls().

Project Submission

For turning in your assignment, push your updated code to the master branch in your repository.

Assignment 5: Inverse Kinematics

Due 11:59pm, Friday, November 17, 2017

For this assignment, you will now control your robot to reach to a given point in space through inverse kinematics for position control of the robot endeffector. Inverse kinematics will be implemented through gradient descent optimization with both the Jacobian Transpose and Jacobian Pseudoinverse methods, although only one will be invoked at run-time.

As shown in the video below, if successful, your robot will be able to continually place its endeffector (indicated by the blue cube) exactly on the reachable target location (indicated by the green cube), regardless of the robot's specific configuration:

The core of this assignment is to complete the kineval.iterateIK() function in the file kineval/kineval_inverse_kinematics.js. This function is invoked within the function kineval.inverseKinematics() with three arguments:

From these arguments and the current robot configuration, the kineval.iterateIK() function will compute controls for each joint. Upon update of the joints, these controls will move the configuration and endeffector of the robot closer to the target.

kineval.iterateIK() should also respect global parameters for using the Jacobian pseudoinverse (through boolean parameter kineval.params.ik_pseudoinverse) and step length of the IK iteration (through real-valued parameter kineval.params.ik_steplength). Kineval also maintains the current endeffector target information in the kineval.params.ik_target parameter.

IK iterations can be invoked through the user interface by holding down the 'p' key. Further, the 'r'/'f' keys will move the target location up/down. When performing IK iterations, the endeffector and its target pose will be rendered as cube geometries in blue and green, respectively.

In implementing this IK routine, please remember the following:

Students enrolled in EECS 398 will implement inverse kinematics for only the position of the endeffector.

IK Random Trial

All students in the AutoRob course are expected to run their IK controller with the random trial feature in the KinEval stencil. The IK random trial is executed through the function kineval.randomizeIKtrial() in the file "kineval/kineval_inverse_kinematics.js". This function is incomplete in the provided stencil. Code for this function to properly run the random trial will be made available upon request through the course discussion channel.

Graduate Section Requirement

Students enrolled in the graduate section of AutoRob will implement inverse kinematics for both the position and orientation of the endeffector, namely for the Fetch robot. The default IK behavior will be for endeffector position control. Both endeffector position and orientation are controlled when the boolean parameter kineval.params.ik_orientation_included is set to true.

Advanced Extensions

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by reaching to 100 targets in a random trial within 60 seconds. A video of this execution must be provided to demonstrate this achievement.

Of the 4 possible advanced extension points, four additional points for this assignment can be earned by implementing the Cyclic Coordinate Descent (CCD) inverse kinematics algorithm by Wang and Chen (1991). This function should be implemented in the file "kineval/kineval_inverse_kinematics.js" as another option within the function kineval.iterateIK().

Of the 4 possible advanced extension points, four additional points for this assignment can be earned by extending your IK controller to use a potential field to avoid collisions.

Project Submission

For turning in your assignment, ensure your completed project code has been committed and pushed to the master branch of your repository.

Assignment 6: Motion Planning

Due 11:59pm, Friday, December 8, 2017

For this assignment, you will now implement a collision-free motion planner to enable your robot to navigate from a random configuration in the world to its home configuration (or "zero configuration"). This home configuration is where every robot DOF has a zero value. For planning, configuration space includes the state of each joint and the global orientation and position of the robot base. Thus, the robot must move to its original state at the origin of the world. Motion planning will be implemented through the RRT-Connect algorithm (described by Kuffner and LaValle).

The graduate section will additionally implement the RRT-Star motion planner of Karaman et al. (ICRA 2011).

The core of this assignment is to complete the robot_rrt_planner_init() and robot_rrt_planner_iterate() in the provided kineval_rrt_connect.js stencil. For successful execution, your implementation of RRT-Connect, the provided collision detection system, and a single specification of world geometry has been included in home.html:

< script src="kineval_rrt_connect.js" ></script> 
< script src="kineval_collision.js" ></script> 
< script src="worlds/world_basic.js" ></script>

The code stencil will automatically load a default world. A world can also be specified as an appended parameter within the URL, in the form of "?world=worlds/world_name.js". The result of including a world file are the global objects "robot_boundary" descrbing the min and max values of the world boundaries along the X, Y, and Z axes and "robot_obstacles" as the locations and radii of sphere obstacles. To ensure these worlds rendered in the display and available for collision detection, the geometries of the world are included through the provided call to kineval.initWorldPlanningScene() in kineval/kineval.js.

Note: your planner should be constrained such that the search does not consider configurations where the base is outside the X-Z plane. Specifically, the base should not translate along the Y axis, and should not rotate about the X and Z axes.

First step: add collision detection

Your RRT-Connect implementation will depend on detection of collisions (provided by the function kineval.robotIsCollision() in kineval_collision.js) with respect to a specified world geometry. Worlds are specified as a rectangular boundary and sphere obstacles. A collection of worlds are provided in the "worlds/" subdirectory of kineval_stencil. The collision detection system performs two forms of tests: 1) testing of the base position of the robot against the rectangular extents of the world, which is provided by default, and 2) testing of link geometries for a robot configuration against spherical objects, which depends on code you will write. Collision testing for links in a configuration is performed by AABB/Sphere tests that require the bounding box of each link's geometry in the coordinates of that link. This bounding box is computed by the following within the loop inside kineval.initRobotLinksGeoms() in kineval.js:

  // bounding box of robot link in local link coordinates
  robot.links[x].bbox = new THREE.Box3;
  robot.links[x].bbox = 

Even before your planner is implemented, you can use the collision system interactively with your robot. The provided kineval.robotIsCollision() function will test the current configuration of the robot. When the robot is detected to be in collision, one of the colliding links will be highlighted with a red wireframe. There could be many links in collision, but only one will be highlighted.

The call to kineval.robotIsCollision() has been placed within my_animate() in home.html:

    // show if robot is currently in collision

Updating kineval_collision for your implementation

To complete the collision system, you will need to modify the forward kinematics calls in kineval/kineval_collision.js. Specifically, you will need to perform a traversal of the forward kinematics of the robot for an arbitrary robot configuration within the function kineval.poseIsCollision(). kineval.poseIsCollision() takes in a vector in the robot's configuration space and returns either a boolean false for no detected collision or a string with the name of a link that is in collision. As a default, this function performs base collision detection against the extents of the world. For collision detection of each link, this function will make a call to function that you create called robot_collision_forward_kinematics() to recursively test for collisions along each link. Your collision FK recursion should use the link collision function, collision_FK_link(), provided below along with a joint traversal function properly positions the link and joint frames for the given configuration.

function collision_FK_link(link,mstack,q) {

  // this function is part of an FK recursion to test each link 
  //   for collisions, along with a joint traversal function for
  //   the input robot configuration q
  // this function returns the name of a robot link in collision
  //   or false if all its kinematic descendants are not in collision

  // test collision by transforming obstacles in world to link space
  mstack_inv = numeric.inv(mstack);
  // (alternatively) mstack_inv = matrix_invert_affine(mstack);

  var i; var j;

  // test each obstacle against link bbox geometry 
  //   by transforming obstacle into link frame and 
  //   testing against axis aligned bounding box
  for (j in robot_obstacles) {

    var obstacle_local = 

    // assume link is in collision as default
    var in_collision = true;

    // return false if no collision is detected such that
    //   obstacle lies outside the link extents 
    //   along any dimension of its bounding box
    if (
      in_collision = false;

    if (
      in_collision = false;

    if (
      in_collision = false;

    // return name of link for detected collision if
    //   obstacle lies within the link extents 
    //   along all dimensions of its bounding box
    if (in_collision)

  // recurse child joints for collisions, 
  //   returning name of descendant link in collision
  //   or false if all descendants are not in collision
  if (typeof link.children !== 'undefined') { 
    var local_collision;
    for (i in link.children) {
       // STUDENT: create this joint FK traversal function 
       local_collision = 
       if (local_collision)
         return local_collision;

  // return false, when no collision detected for this link and children
  return false;

kineval_collision.js uses matrix and quaternion calls based on the reference implementation (i.e., the instrutor's code). Your matrix and quaternion calls likely have a different structure to the function arguments and returned data structures. You should either:

You can feel free to implement matrix_invert_affine() instead of using numeric.inv(). Affine transforms can be inverted (in constant time, Quiz 3!) through a much simpler process than the generic matrix inversion, which is O(n^3) for Gaussian elimination.

If successful to this point, you should be able to see the collision world of the robot, move around this world, and see the colliding link display a red wireframe when a collision occurs.

Implementing and invoking the planner

Your motion planner will be implemented in the file kineval/kineval_rrt_connect.js through the functions kineval.robotRRTPlannerInit() and robot_rrt_planner_iterate(). The kineval.robotRRTPlannerInit() function should be modified to initialize the RRT trees and other necessary variables. The robot_rrt_planner_iterate() function should be modified to perform a single RRT-Connect iteration based on the current RRT trees. Basic RRT tree support functions are provided for initialization, adding configuration vertices (which renders "breadcrumb" indicators of base positions explored), and adding graph edges between configuration vertices. This function should not use a for loop to perform multiple planning iterations, as this will cause the browser to block and become unresponsive. Instead, the planner will be continually called asynchronously by the code stencil until a motion plan solution is found.

Once implemented, your planner will be invoked interactively by first moving the robot to an arbitrary non-colliding configuration in the world and then pressing the "m" key. The "m" key will request the generation of a motion plan. While the planner is working, it will not accept new planning requests. Thus, you can move the robot around while the planner is executing.

Planner output

The output of your planner will be a motion path in a sequentially ordered array (named kineval.motion_plan[]) of RRT vertices. Each element of this array contains a reference to an RRT vertex with a robot configuration (.vertex), an array of edges (.edges), and a threejs indicator geometry (.geom). Once a viable motion plan is found, this path can be highlighted by changing the color of the RRT vertex "breadcrumb" geom indicators. The color of any configuration breadcrumb indicator in a tree can be modified, such as in the following example for red:

  tree.vertices[i].geom.material.color = {r:1,g:0,b:0};

The user should should be able to interactively move the robot through the found plan. After adding the following code below to user_input() in kineval_userinput.js, the "n" and "b" keys to move the robot to the next and previous configuration in the found path, respectively. These user key presses will respectively increment and decrement the parameter kineval.motion_plan_traversal_index such that the robot's current configuration will become:


Note: we are NOT using robot.controls to execute the found path of the robot. Although this can be done, the collision system does not currently test for configurations that occur due to the motion between configurations.


Make sure to test all provided robot descriptions from a reasonable set of initial configurations within all of the provided worlds, ensuring that:

Graduate Section Requirement

In addition to the requirements above, students in the graduate section must also implement the RRT-Star motion planning algorithm and test to ensure:

Advanced Extension

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by adding the capability of motion planning to an arbitrary robot configuration goal.

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by using the A-star algorithm for base path planning in combination with RRT-Connect for arm motion planning.

Of the 4 possible advanced extension points, one additional point for this assignment can be earned by writing a collision detection system for two arbitrary triangles in 2D using a JavaScript/HTML5 canvas element.

Of the 4 possible advanced extension points, two additional points for this assignment can be earned by writing a collision detection system for two arbitrary triangles in 3D using JavaScript/HTML5 and threejs or a canvas element.

Of the 4 possible advanced extension points, four additional points for this assignment can be earned by implementation of triangle-triangle tests for collision detection between robot and planning scene meshes.

Warning: Respect configuration space

The planner should produce a collision-free path in configuration space (over all robot DOFs) and not just the movement of the base on the ground plane. If your planner does not work in configuration space, it is sure to fail tests used for grading.

Highly recommended: start with HTML5 Canvas Stencil

Using the browser for as a development environment has many benefits. However, when coding mistakes occur, it will make the browser lock up and be completely unusable. Such mistakes can be especially difficult to debug when the overhead of rendering with threejs is involved.

To help you get started, the path planning code stencil in the "search_canvas" directory has entry points for developing your core RRT routines. This stencil will allow you to implement the RRT-Connect algorithm in simplified 2D worlds with provided routines for visualization and collision. Because the RRT is invariant across configuration spaces, an RRT developed for the 2D Canvas world should easily port to the N-D threejs world, with minor changes for invoking drawing routines.

Project Submission

For turning in your assignment, ensure your completed project code has been committed and pushed to the master branch of your repository.

Assignment 7: The best use of robotics?

Slides due 11:59pm, Friday, December 8, 2017
Presentation due 1:30pm, Monday, December 11, 2017

Scenario: An investor is considering giving you 20 million dollars (cold hard USD cash, figuratively). This investor has been impressed by your work with KinEval and other accomplishments while at the University of Michigan. They are convinced you have the technical ability to make a compelling robot technology... but, they are unsure how this technology could produce something useful. Your task is to make a convincing pitch for a robotics project that would yield a high return on investment, as measured by some metric (financial profit, good for society, creation of new knowledge, etc.).

You will get 2 minutes to make a pitch to develop something useful with robots. Consider the instructor and your classmates as the people that need to be convinced. As a guideline, your pitch should address an opportunity (presented by a need or a problem), your planned result (as a system, technology, product, and/or service), and how you will measure successful return on investment. Return on investment can be viewed as financial profit (wrt. venture capital), good for society (wrt. a government program), creation of new knowledge or capabilities (wrt. a grant for scientific research). Remember, the purpose is to convince and inspire about what is possible, rather than dive into specifics.

The last scheduled class period and a little more (Monday December 11th, 1:30-4:30pm) will be dedicated to student presentations to pitch ideas on the best use of robotics.

Please post your slides to the "#asgn7-best-use" discussion channel before 11:59pm on Friday December 8th. Your first slide must include the title of your presentation, your name, and your UID. The filename of your slides must start with your UID in the format "asgn7_UID". Slides will only be accepted in PDF format, although embedding of videos or links to videos will be accepted. You can post new versions of your slides up to the submission deadline on December 8th.

The pitch judged to be the most convincing will get first dibs.

Additional Materials

Appendix: Git-ing Started with Git

Using version control effectively is an essential skill for both the AutoRob course and, more generally, contributing to advanced projects in robotics research and development. git is arguably the most widely used version control system at current. Examples of the many robotics projects using git include: Lightweight Communications and Marshalling, the Robot Operating System, Robot Web Tools, Fetch Robotics, the NASA Robonaut 2, and the Rethink Baxter. To help you use git effectively, the course staff has added the tutorials below for getting started with git. This is meant to be a starting guide to using git, bash, and Git Bash. For a more complete list of commands and features of git, you can refer to the following guides: The Git Pro book or The Basic git command line reference for windows users. An interactive tutorial for git is available at LearnGitBranching.

Installing git

The AutoRob course assumes git is used from an command line terminal to work with a git hosting service, such as GitHub or Bitbucket. Such terminal environments are readily available in Linux and Mac OSX through their respective terminal programs. For MS Windows, we are recommending Git Bash, although several other viable alternatives exist. Applications that provide a Graphical User Interface for git are not recommended.

git can be installed on Linux through a common package managment system, based on your distribution, with one of the following commands:

sudo yum install git-all

sudo apt-get install git-all

For Mac OSX, git can be installed on its own using the Git-OSX-Installer or as part of larger set of Xcode build tools. For MS Windows, we are recommending Git Bash, although several other viable alternatives exist.

If you have a command line terminal running, you should see a shell environment that looks something like this (screenshot from Git Bash):

If you have git installed, you should should be able to enter the "git" command and see the following usage information printed (screenshot from OSX):

Cloning your repository

The most common thing that you will need to do is pull and push files from and to your git hosting service. Upon opening Git Bash, you will need to go to the location of both your GitHub/Bitbucket repository on the web and your git workspace on your local computer. Our first main step is to clone your remote repository onto your local computer. Towards this end, the next step is to open your terminal application and determine your current directory, assuming you will use this directory to create a workspace. For Linux and OSX, the terminal should start in your home directory, often "/home/username" or "/Users/username". For Git Bash on Windows, the default home directory location could be the Documents in your user directory, or the general user folder within "C:\Users".

From your current directory, you can use Bash commands to view and modify the contents of directories and files. You can see a list of the files and folders that can be accessed using ls (list) and change the folder using the command cd (change directory) as shown below. If you believe that the directory has files in addition to folders, but would like a list of just the folders, then the command ls –d */ can be used instead of ls. Below is a quick summary of relevant Bash commands:

You are now ready to clone a copy of your remote repository and populate it with files for AutoRob projects. It assumed that you have already created a repository on your git hosting service, given the course staff access to this repository, and provided a link of your repository to the course staff. This repository link (in the form of "") will now be used to clone a copy of your remote repository onto your local machine using the following git command below. This command will clone the repository contents to a subdirectory labeled with the name of the repository:

  git clone [repository URL link]

This directory should be listed and inspected to ensure it has been cloned with the contents of the repository, matching the remote repository from your git hosting service. If this is a new repository, it is not problem for this directory to be empty:

  ls [repository_name]

You can also check for differences between the files on your computer and the remote repository using git status as shown below. If you receive the message shown in the example below, then there are no differences. If there are differences, then it will have the number of files which are different highlighted in red.

$ git status
On branch master
Your branch is up-to-date with 'origin/master'.
nothing to commit, working directory clean

Important: workspace is not the same as repository

You should now have a local copy of your repository with a workspace in a subdirectory. It is critical to note that your local repository is different than the subdirectory with your current workspace. Your workspace is not automatically tracked by the version control system and considered ephemeral. Any changes made to your workspace must be committed back into the local repository to be recognized by the version control system. Further, any changes committed to your local repository must also be pushed remotely to be recognized by your git hosting service. Thus, any changes made to your workspace can be lost if not committed and pushed, which will be discussed more in later sections.

Populating your repository with project stencil code

In a separate directory, clone the kineval-stencil-fall16 repository to a subdirectory on your local machine:

  cd [home_directory]
  git clone

Inspect this directory to ensure it has been cloned with the contents of the repository:

  cd kineval-stencil-fall16

and open "home.html" from this directory in a web browser and ensure you see the starting point picture below:

Then, copy the kineval-stencil-fall16 files to the directory with your workspace

  cd [home directory]
  cp -r kineval-stencil-fall16/* /

As these copied files are new to your working repository, they need to be added to the repository to ensure they are tracked for version control. These files are added with the following commands:

  cd [repository name]
  git add * 

Below is a more detailed summary of git commands for adding files from your workspace to your repository:

Commit and push to update your repository

Whenever you make any significant changes to your repository, these changes should be committed to your local repository and pushed to your remote repository. Such changes can involve adding new files or modifying existing files in your local workspace. For such changes, you will first commit changes from your workspace to your local repository using the git commit command:

git commit -a -m "message describing changes"

and then pushing these changes from your local repository to a synced repository on your git hosting service:

git push

This commit will occur to the "master" branch of your repository.

Note: the change files must be located in the correct repository folder on your local computer and these commands should be performed in the local workspace directory. Below is a more detailed summary of git commands for adding files from your workspace to your repository:

Once you have committed and pushed, your local workspace becomes redundant as your changes have been stored and tracked remotely. The local workspace can now be deleted without concern. This local workspace can also be updated with changes to the remote repository by pulling.

Pulling remote changes

Changes be made to your remote repository, potentially by other collaborators, without being tracked by your local repository. This can lead to potential versioning conflicts when committed changes contradict each other. For the AutoRob course, versioning conflicts should not be a problem because commits to your repository should be yours alone. That said, one good practice is to ensure your workspace, local repository, and remote repository are synced before making any changes. A brute force method for doing this is to re-clone your repository each time you begin to make changes. Another option is to pull remote changes into your local repository and workspace using the git pull command (or git fetch command):

cd [repository_name]
git pull

Below is a more detailed summary of git commands for pulling and fetching:


Branching is an effective mechanism for work in a repository to be done in parallel with changes merged at a later point. A branch essentially creates a copy of your work at a particular version. Branches are independently tracked by the version controller and can be merged together when requested (which collaboratively results in a "pull request"). The larger story for branching and merging is outside the scope of AutoRob.

The working version of your code, which you submit for grading, is expected to be in the "master" branch of your repository. When working on a new assignment, it is recommend that you create a branch for this new work. This allows your stable code in the master branch to be undisturbed while you continue to modify your code. Once your work for this assignment is done, you can then update your master by merging in your assignment branch. Stylistically, it is helpful to use the name Assignment-X for your assignment branch for Project number X.

The simplest means for branching in this context is to use the branching feature from the webpage of your remote repository. From GitHub, simply select the master branch from the "Branch: " button and enter the name of the branch to be created. From Bitbucket, select the "Branches" icon from the left hand toolbar and follow the instructions for branch creation. If successful, you should see a list of branches that can each be inspected for their respective contents. Branches can also be deleted from this interface.

A branch can also be created from the command line by the following, which will create a copy of the current branch:

git branch [branch_name]

You can switch between branches with the following command:

git checkout [branch_name]

as well as clone a specific branch from a repository:

git clone -b [branch_name] [repository URL link]

Good luck and happy hacking!