HW3: Modeling a robotic finger
Tomasz Malisiewicz, Mark Palatucci, and Geoff Hollinger
Abstract
For this assignment, we modeled a robotic finger. The assignment page describes the task. To perform this task, we developed an ODE simulation of the finger dynamics and performed a unifrom grid search to determine the required torques to draw a 2D square with the finger tip. The constraint between the second and third joint was handled by doing an auxilliary search at each iteration. Torques were only applied if they satisfied this constraint. This technique performed well at the task of controling the finger. We verified our results using a simulation built on the Matlab robotics toolbox.
Finger Modeling
We modeled our robotic finger in ODE by modifying the double pendulum simulation from assignment 2. We changed the box links from the previous assignment to cylinders of the specified dimensions. A third link (and joint) was added, and the ability to apply torque to that joint was coded in. The position of the straight finger was pointed downward, and the joints were allowed to move to the right side of the screen. This models a downward pointed left forefinger viewed from the back. Functionality was added to the simulation to allow the finger to start in varying initial conditions (initial velocities and angles of the first two joints). Additionally, constraints were added to keep the finger at acceptable angle configurations. When controlling the finger, however, these constraints were dropped (see the following sections). The animation below gives the final trajectory of the finger. The technique for developing this trajectory is described in subsequent sections.
Defining the Square in Accessible Space
The first task was to define the corners of the square (in cartesian space) that are accessible by the robotic finger. This was performed by uniformly sampling the space of all configurations [200 x 200 divisions] (measured by the angles of the joints) and performing some basic statistical analysis on the resulting positions of the finger tip. Our hypothesis was that the mean and covariance of the finger tip positions would define a high-accessability ellipsoid in cartesian space. We simply aligned the square with these first and second order moments. Alignment was performed by performing an eigenvector decomposition of the covariance matrix.
After obtaining the corners of this square, we decided to equally partition the sides of the square to produce a list of desired finger tip positions. We used [50] divisions per square side. Since we would follow each desired finger tip position, for the time delta used in the ODE simulation this defined our trajectory to be 2 seconds. Then for each finger tip position, we searched over all the configuration angles that we sampled earlier to find the nearest configuration (in theta space) corresponding to the desired point (in cartesian space). In essence, this was a way of doing inverse kinematics by nearest neighbor lookup. At this point we had a desired trajectory in cartesian space (that sits perfectly on the square that we defined above) and for each cartesian point an approximate theta configuration (a trajectory in theta space).
The above plot shows a random subsample of the cartesian finger tip positions in yellow. A few random robotic finger configurations are displayed in red with the first joint in blue, second joint in magenta, and finger tip in green. The mean of the accessability region is displayed as a large green start, the ellispoid is displayed in blue, and the final square is displayed in blue.
Following a Trajectory Using Uniform Search
Once we obtained a desired trajectory and an initial configuration in angle space, we simply started our simulation in this initial configuration. In order to obtain a set of 3 torques that would trace our square and satisfy the constraint between the last two angles, we used a uniform grid search.
At each iteration of the simulation, we search over the space of torques to find the single best vector of torques that brings us to the next desired configuration. During the search, we uniformly sample a two-dimensional region of torques centered at (0,0). For each set of two torques, we then uniformly search over the third torque to find the third torque that keeps the angular constraint satisfied. In our simulation we never explicitly enforce the constraint between the last two angles and have to perform this auxillary search. We then see where an application of those three torques moves us to. We have a desired position in euclidean space that we want to move to, so we just find the best set of torques at each time step. Best torques are defined as the ones that bring the finger tip closest to the next desired state.
The above figure shows the trajectory that our robotic finger followed as well as the desired trajectory. Clearly, performing uniform grid search proved sufficient for controlling the robotic finger.
The above figure shows the deviation (measured as euclidean distance from finger position to desired finger tip position) of our robotic finger across time.
Trajectory Files
Here are links to the two trajectory files produced by our code
Verification using MATLAB Robotics Toolbox
We used the MATLAB Robotics Toolbox to verify the simulation. The toolbox works by manually defining the robot dynamics using
a matrix of parameters:
Each row of the matrix defines a link in the robot and contains certain properties such as mass, moments,
center of gravity, and axis of rotation. We pulled these parameters directly from the ODE simulation to make sure
there were no differences. Once configured, we plotted our robot with the same initial condition as our ODE
simulation:
We first verified the angle and coordinate frames by explicitly setting the joint angles. A nice movie is
here.We then tried to verify the feedfoward torques from ODE in the robot toolbox.
We didn't expect that the ODE torques would properly drive the toolbox robot. The toolbox simulation uses a completely
different integration technique to calculate the forward dynamics. Using just an open loop with the ODE torques, we
found the the toolbox simulation would follow the path for about one length of the square, but then it would diverge from
the path. We spent many hours trying to resolve the differences without much success. We believe the primary problem is that
the toolbox uses Runge-Kutta45 to compute the integration - and this method does not have the same concept of uniform timesteps
that ODE has.
Code
The link below gives the code for this assignment (ass3kdc.cpp gives the initial simulation, and ass3kdc2.cpp gives the simulation with the grid search):