Assignment 1: Wheeled Robot Control

Part 1: Kinematics

In the first part of this assignment, you will implement models for the forward and inverse kinematics of a differential drive robot. This assignment will assume the following conventions:

Poses will consist of x and y coordinates (measured in meters) and a theta coordinate (in radians).
Each atomic Action will be rotating the left and right wheels at a given speed for some set period of time. The wheel velocity will be measured in radians/s and time will be given in seconds.
The robot will be described by three parameters. First, the axle length will be the distance between the centers of the wheels in meters. Second, the wheel radius is the radius of the wheels. Lastly, we have a maximum speed at which the wheels can rotate in radians per second, which is only used in the inverse kinematics.

Pure Python

The heavy mathematical lifting for calculating kinematics in this assignment will be done in assignment01/src/assignment01/kinematics.py.

The first function to define is for forward kinematics.

def forward(p, a, rd):
    (x,y,theta) = p
    (vl,vr,t) = a
    (axle_length, wheel_radius, max_speed) = rd
    return 0.0, 0.0, 0.0

Given a pose and an action (given as 3-tuples) and a robot description (2-tuple), return the new pose.

The second function defines inverse kinematics.

def inverse(p0, p1, rd):
    (x0,y0,theta0) = p0
    (x1,y1,theta1) = p1
    (axle_length, wheel_radius, max_speed) = rd
    
    return []

Given two poses and a robot description, return an array of actions that will move the robot from one pose to the other. In the simplist cases, the robot will only need a single action. However, some pose combinations will require multiple moves.

There are three possible solutions for our robot. The first is the easiest to implement, that uses three discrete actions per calculation: turn, move in a straight line, turn. The second approach is for positions that only require one move, and is just the inverse of the forward kinematics. The third approach is something that takes fewer than three moves to move anywhere.

To test the code, you are given two python scripts. assignment01/src/forward.py will call your forward kinematic function based on command line arguments and print out the resulting pose. assignment01/src/inverse.py will call your inverse kinematic function based on command line arguments, print out the resulting set of actions, and use your forward kinematic model to figure out where the plan actually ends up.

ROS Python

Using the first half of the rospy service tutorial as a guide, write a python script in assignment01/src/services.py that creates Services for both the forward and inverse kinematics, as defined in wheeled_robot_kinematics/srv.

You need to add the following line to the top of your code, before any of the other imports:
```
import roslib; roslib.load_manifest('assignment01')
```
Initialize your node (rospy.init_node) with your name/team name as the parameter/name.
The two services should be called "~forward" and "~inverse". This will make the full name of your service /teamname/forward.
Be sure to use your libraries from the first part.
The robot description parameters should be read from the parameter server with the names /axle_length, /wheel_radius and /max_speed.
Note that you can also build your response by accessing the fields of the response like this:
```
resp = DiffDriveFKResponse()
resp.end_pose.x = x
...
return resp
```

You may also want to build your IK response in a similar way.

resp = DiffDriveIKResponse()
resp.actions.append( DiffDriveAction(lv, rv, t) )

Make sure your script is executable.

This code can be tested on cse550.com.

While in the assignment01 package, make your packages.
```
rosmake
```
In one terminal, run your services script (roscore should already be running)
```
rosrun assignment01 services.py
```
In another terminal, run the tests provided by the kinematics package, with your teamname as a parameter.
```
rosrun wheeled_robot_kinematics kinematics_test.py teamname
```
This should show you a number of tests and their results.

Simulation Test

With the IK service implemented, you can begin to command a robot to particular locations. To see this in action, we can run a simulated diff-drive robot in the Gazebo simulator.

In one window, run
```
roslaunch wheeled_robot_sim demo.launch
```
Note that you can also append parameters onto the end to say what size robot you'd like to generate.
```
roslaunch wheeled_robot_sim demo.launch axle:=1.25 radius:=.5
```
In a second terminal, start your services script.
```
rosrun assignment01 services.py
```
In a third window, run the drive.py script. You will need to specify your team name from above, as well as the coordinates you'd like the robot to drive to.
```
rosrun wheeled_robot_sim drive.py
```
```
usage: drive.py [-h] [-x X] [-y Y] [-theta THETA] name
```

Note: This cannot be run on cse550.com.

Part 2: Controllers

In this part of the assignment, your goal is to program a robot to drive in an aribitrary poylgon shape. The polygon will depend on two parameters: the number of sides to the polygon, and the maximum dimension of the shape. As the number of sides increases, the length of each side decreases.

In general, the algorithm will be

Drive distance d.
Turn a radians.
Goto 1.

Pure Python

In assignment01/src/assignment01/polygon.py, write two functions.

def get_angle(sides)

and

def get_side_length(sides, max_d)

which return an angle a in radians and a distance d in meters for the polygon.

ROS Python

Using the above library, write two scripts to drive in a polygon shape.

The first version of the algorithm will be open loop in assignment01/src/open_polygon.py. It will figure out what velocity it needs to move for the specified angle/distance, and drive that velocity for a set period of time.

The second version of the algorithm will have proportional control in assignment01/src/prop_polygon.py. It will use its current position to figure out its next action.

To drive the robot, you will need to publish geometry_msgs/Twist messages, setting linear.x for forward motion and angular.z for turning. Publish on the /cmd_vel_mux/input/navi topic. Note that the top linear speed is 0.5 m/s and the top angular speed if 0.5 radians/s.
To manage the time aspect of the first script, use the rospy Time library. For instance, one way to publish a message for a set amount of time is as follows:
```
r = rospy.Rate(10)
t = rospy.Time.now()

while rospy.Time.now() - t < rospy.Duration(number_of_seconds):
    pub.publish( msg )
    r.sleep()
 
```
To get the robot's current position in the second script, subscribe to the odom topic to recieve messages of type nav_msgs/Odometry. You only need to look at the msg.pose.pose part of the message. To convert between quaternions and the base angle, use tf.transformations.euler_from_quaternion.

On a Turtlebot

To demonstrate that your code works, run the code on a turtlebot. A demo of this must either be recorded on video (and an uploaded link sent to the professor) or done live for the professor.

Grading

Forward method	15 points
Inverse method (general)	10 points
Single-move IK	5 points
Minimal IK	bonus 10 points
Forward Service	10 points
Inverse Service	10 points
get_angle and get_side_length methods	5 points
Open Loop	15 points
Proportional Control	20 points
Polygon Demo	10 points

Due Date: February 15, 2015, 11:59pm CST.

CSE550

Mobile Robotics

Assignment 1: Wheeled Robot Control

Part 1: Kinematics

Pure Python

ROS Python

Simulation Test

Part 2: Controllers

Pure Python

ROS Python

On a Turtlebot

Grading