Problem set: differential kinematics

Spiderbot

Spiderman’s coordinates in the New York City skyline above are \((x, y)\). (He’s a point spider.) His configuration is \((l_1, l_2)\), the lengths of the two strands of spider silk. You may assume that the two points where the strands connect to the buildings are the same height, and choose coordinates so that this height is zero. Choose units so that the distance between the two buildings is 1/2.

1a) Write the forward kinematics of spiderman, giving the mapping from \(l_1, l_2\) to \((x, y)\). Big hint from MathWorld: the intersection of two circles, one of which has a center at the origin, and the other of which has a center at the coordinates (d, 0), with radii \(R\) and \(r\) respectively has \(x\) coordinate \(x= \frac{d^2 - r^2 + R^2}{2d}\) and \(y\) coordinate \(y = \sqrt{\frac{4 d^2R^2 - (d^2 - r^2 + R^2)^2}{4d^2}}\).

1b) Write the general differential kinematics of spiderman, that gives the mapping from \((\dot l_1, \dot l_2)\) to \((\dot x, \dot y)\) in terms of \(l_1, l_2\). Full credit will only be given if you write it in matrix form.

1c) Write the inverse kinematics of spiderman, which compute \(l_1\) and \(l_2\) for a given \(x, y\) configuration. Hint: inverse kinematics aren’t always difficult. Look at the geometry and draw some triangles rather than trying to solve the equations.

Drawbot

A differential-drive robot like the Turtlebot Burger steers using difference between the wheel speeds to turn. Although the configuration space has three dimensions (perhaps described by \(x\) and \(y\) of the center, and angle of the robot), there are only two wheel speeds that may be controlled. This means that there are some directions that the robot cannot immediately drive in, a non-holonomic constraint. For example, the center of the robot cannot move sideways.

Imagine we attach a pen to the turtlebot so that it can draw on the floor. Where should we attach the pen? If we put the pen at the center of the robot, the pen can only move forwards and backwards or spin, making it hard to draw smooth curves.

Let’s pen out front of the robot somewhere on the perpendicular bisector of the line segment that connects the contact points of the wheels with the ground. Then, if the robot drives forwards, the pen moves forwards. If the robot spins counter-clockwise, the pen moves to the left.

2a) Draw a picture of this situation to show that you understand the pen placement and how moving the robot will move the pen. You may choose coordinates however you like, but indicate how many centimeters you’ve chosen to put the pen out front of the turtlebot.

2b) Forward kinematics. Write down equations \(x_p(x, y, \theta)\) and \(y_p(x, y, \theta)\) that express the pen location as a function of the robot configuration.

2c) Differential kinematics. Write down the Jacobian matrix \(J\) such that \(J \dot q = \dot p\), where \(q = (x, y, \theta)\), \(J\) is a function of \(q\), \(p = (x_p, y_p)\), and the dot operator indicates taking a time derivative. (Hint: take some partial derivatives of the forward kinematics equations.)

2d) Controlling the pen. By setting the wheel velocities of the left and right wheels, the velocity of the robot in the forward direction can be chosen. Specifically:

\(v = r \frac{\omega_1 + \omega_2}{2}\), if \(\omega_1\) and \(\omega_2\) represent the angular velocity of each wheel and \(r\) is the radius of each wheel. (Basically, the robot velocity is the average of the two wheel velocities.

Similarly, the angular velocity of the robot \(\omega\) may be computed as:

\(\omega = r \frac{\omega_1 - \omega_2}{2b}\), if \(b\) is the distance between the wheels, and wheel 2 is the left wheel.

So, if we choose the wheel speeds \(\omega_1\) and \(\omega_2\), we may compute the velocity of the robot: \(v\) and \(\omega\). Once we know \(v\) and \(\omega\), we can compute the rate of change of the configuration of the robot: \(\dot x = v \cos \theta\), \(\dot y = v \sin theta\), and \(\dot theta = \omega\).

Combining these results with your work for 2c, you could compute the velocity of the pen a a function of the wheel velocities. But that’s not what we want! We want the inverse: we want to pick the motion of the pen, and then compute the needed motion of the wheels to move the pen in the desired direction. For example, let’s say you wanted to draw a horizontal line with the pen. At any instant, we need to compute what velocities to command the wheels; depending on the robot configuration, these wheel velocities might be different.

Write a set of equations that compute \((\omega_1, \omega_2)\) as a function of the desired \(\dot x_p, \dot y_p\). Hint: organize the equations described above to compute a matrix equation computes pen motions when given wheel velocities. Then invert the matrix. (You need not explicitly invert the matrix; you can leave the superscript \(-1\) in the equation.

2d) Write a simulator that draws motions of the robot as the robot draws a horizontal line, or some other shape. Take some screenshots of the simulator in action. (I’d recommend Matlab or Python. matlplotlib might be sufficient for the drawing. numpy also might be useful for the matrices.)

Robot arm geometry

3a) Estimate the Jacobian at the current configuration. Don’t do any computations; just eyeball or measure the configuration and write down your estimate.

3b) Assume \((\dot \theta_1, \dot \theta_2, \dot \theta_3, \dot \theta_4) = (1, 1, 3, 1)\). What is your estimate for \((\dot x, \dot y)\)?

3c) In terms of sines and cosines of the joint angles, and the lengths of the links, write down the analytical Jacobian by taking partial derivatives.