Extending
Tekkotsu to New
Platforms
Creating
a Kinematics File
and Forward Kinematics Support
The
first step towards performing cognitive robotics with the arm was
to create a kinematics descriptor file. With this, we could accomplish
two major goals. First, we could use forward kinematics and solve for
where the gripper is in terms of the base's coordinate frame. Second,
and more importantly, we could use inverse kinematics and position the
joints in order for the gripper to reach a certain point.
To
construct the kinematics file, we used the following procedure from
Robert Schilling's Fundamentals
of Robotics: Analysis and Control:
0.
Number the joints from
1 to n
starting with the base and ending with the tool yaw, pitch, and roll,
in that order.
1. Assign a right-handed orthonormal coordinate frame L0 to
the robot base, making sure that z0 aligns
to the axis of joint 1. Set k = 1.
2. Align zk
with the axis of joint k + 1.
3. Locate the origin of Lk
at the intersection of the zk
and zk+1
axes. If they do not intersect, use the
intersection of zk
witha common normal between zk
and zk-1.
4. Select xk
to be orthogonal to both zk
and zk-1.
If zk
and zk-1
are parallel, point xk
away from zk-1.
5. Select yk
to form a right-handed orthonormal coordinate
frame Lk.
6. Set k =
k+1. If k < n, go to step 2;
else, continue.
7. Set the origin of Ln
at the tool tip. Align zn
with the approach vector, yn
with the sliding vector, and xn
with the normal vector of the tool. Set k = 1.
8. Locate point bk
at the intersection of the xk
and zk-1
axes. If they do not intersect, use the
intersection of xk
with a common normal between xk
and zk-1 .
9. Compute Θk as
the angle of rotation from xk-1
to xk
measured about
zk-1 .
10. Compute dk as
the distance from the origin of frame Lk-1 to
point bk
measured along zk-1 .
11. Compute ak as
the distance from point bk
to the origin of frame Lk measured
along xk .
12. Compute αk as
the angle of rotation from zk-1
to zk
measured about xk .
13. Set k
= k+1. If k ≤ n, go to
step 8; else, stop.
(Schilling,
page 54)
We
followed this procedure and ended up with a kinematics file where each
entry resembles the following:
[Arm_LINK3]
joint_type: 0
immobile: 0
theta: -1.570796326794895
d: 0.0
a: 154.0
alpha: 0.0
theta_min: -1.570796326794895
theta_max: 1.570796326794895
m: 0.0
cx: 0.0
cy: 0.0
cz: 0.0
Ixx: 0.0
Ixy: 0.0
Ixz: 0.0
Iyy: 0.0
Iyz: 0.0
Izz: 0.0
tekkotsu_output: 2
The
basic parameters are obvious from the Denavit-Hartenburg procedure from
Schilling's book. However, Tekkotsu adds a few other notable fields.
- joint_type is 0 for a rotational joint and 1 for a
prismatic joint (such as the arm's gripper).
- theta_min and theta_max are additive offsets from theta. We
believed that they were also limits for the maximum joint values for
the kinematics solver in ROBOOP, but to our dismay, we discovered that
this was not the case.
- tekkotsu_output corresponds to a motor entry specified
elsewhere in Tekkotsu.
Once
we did this, we could run our forward kinematics solver and see that it worked. One
problem that we ran into was that the servos on the arm were not strong
enough to fully support the arm's weight, so our results were always
slightly off. If we held the arm at the point of zero resistance from
any of the servos, everything would work fine. A related problem was
the fact that we could not use PID control over the servos. Since there
was only one-way communication, we could only control the P value. As a
result, the arm would often start to oscillate and would continue until
we stopped it manually.
Regardless,
forward kinematics were functional if we were to assume that our arm
existed in a perfect world where the servos were always where we told
them to be. It was the best that we could possibly do.