Homework 2

Create a node called trajectory that

1. Publishes a cmd_vel (geometry_msgs/Twist) message at a fixed frequency, causing the turtlebot to follow the desired open-loop trajectory
   • It is up to you to find an appropriate frequency, but 10 Hz is too slow and 1 kHz is faster than needed.
2. Broadcasts:
   • A static transform from world to odom.
   • The world frame and odom frame are co-located. When the turtlebot is at (0, 0, 0) in the odom frame it is at \(0, 0, \theta_0\) in the world frame, where \(\theta_0\) is the initial angle of the robot on its figure-eight trajectory
   • Thus, when the turtlebot starts, it can initiate its odometry to (0,0,0) but always be facing the correct direction in the odometry frame
3. Services:
   • pause (type up to you) Stop the turtle's motion, in a way that the trajectory can be resumed
   • resume (type up to you) Resume the turtle's motion along the trajectory
4. Parameters:
   • width: The width of the figure eight
   • height: The height of the figure eight
   • period: The amount of time it takes to complete the figure eight
   • ~pub_freq: The frequency at which to publish the messages. This is a private parameter.
   • Pick parameters that fit your location and the turtlebot's capabilities.

The purpose of this node is to allow you to use the turtlesim node instead of a real robot while refining your trajectory

Create a node called simodom that

1. Subscribes to a turtlesim/Pose message
2. Publishes the corresponding nav_msgs/Odometry message
   • The header frame is odom
   • The child frame is base_footprint
   • You can leave covariance information as zeros
3. Broadcasts the corresponding transform between odom and base_footprint
4. When the robot first is turned on, it assumes that it is located at (0,0,0) in the /odom frame.
5. The /base_footprint frame is the top-level frame of the turtlebot3 (e.g., all the turtlebot's frames are children of /base_footprint. Therefore, the transform between /odom and /base_footprint corresponds to where the robot thinks it is, relative to where it was when it first was turned on.

A python package called homework2 and located under src/homework2

• The ROS package is $ws/src/homework2 and the python package is $ws/src/homework2/src/homework2, where $ws is your workspace
• This package should contain code that performs calculations related to the trajectory and control but does not interact with the hardware
  • Generally, code that performs calculations is much easier to test and can be used in a wide variety of situations so it makes sense to split it from the node code.
  • This code should not use any rospy functions at all
• At a minimum, the package should provide functions to
  • Compute the trajectory and its derivatives at a given time
  • Compute the proper control inputs at a given time
• The trajectory node shall import the homework2 package to do its calculations and use the results to control the robot

Unit tests for your python package using rossunit

1. You should be able to run the tests from catkin_make run_tests
2. Test each of the following computations at \(t = 0\) and at least one other time \(t\): \(x_d(t)\), \(y_d(t)\), \(\dot{x}_d(t)\), \(\dot{y}_d(t)\), \(\ddot{x}_d(t)\), \(\ddot{y}_d(t)\), \(v(t)\), \(\omega(t)\).
3. For this assignment, it is only necessary to test the functions/methods in the python package, not your nodes.

1. A config directory containing
   1. A trajectory.yaml file containing the trajectory parameters (the publishing frequency is specified directly in the launchfile)
   2. A turtle_view.rviz file containing your rviz setup.
2. A launchfile in the launch/ directory called figure_eight.launch that
   1. Loads the width, height, period parameters from trajectory.yaml
   2. Starts the trajectory node, including setting the pub_freq parameter

   3. Launches rviz

      • When launching rviz it loads turtle_view.rviz to get the rviz settings
      • The fixed frame should be /world
      • The odometry should be displayed (using an rviz odometry visualizer)
      • All the tf frames should be hidden except for /world. However, the tf tree visualizer should be added to the scene (i.e., the left panel of rviz)
      • The robot model should be shown
      1. Provides an option called mode
         • When mode is sim (the default) nodes are launched to command the trajectory in turtlesim, but also to have the robot model in rviz moving as if the turtle were the real robot.
         • You will need to publish joint_states for the wheels for the robot to show up in rviz. These joint states can just be 0, you do not need to simulate the wheels actually turning.
         • When mode is real, the robot with name robot_name runs the trajectory in real life. turtlesim_node should not be started

1. Documentation showing how you computed the control inputs from the trajectory is required. You should link/refer to the documentation in your README.md. A few valid methods of showing the computations are:
   1. Python code using sympy to perform the calculations
   2. A Mathematica Notebook
   3. A picture or scan of handwritten calculations
   4. A pdf of calculations written in latex
   5. Directly embedded in your README.
2. The functions and classes you write for the python package should be commented with docstrings as per the Guidelines
3. The nodes you write should be commented with docstrings as per the Guidelines
4. Write a README.md as per the Guidelines
   • Assume the user already knows how to use ROS and our turtlebots so no need to explain the basic setup.
5. Videos:
   • A screencast of the robot in rviz
   • A video of the robot completing a figure eight in turtlesim.
   • A video of the robot completing a figure eight in real life.
   • Videos can be a link to a sharing site in your README or as a gif file.

1. Solve the kinematic equations for \(\theta = f(\dot{x}, \dot{y})\), (thus \(\theta\) is some function \(f\) that depends only on \(\dot{x}\) and \(\dot{y}\).
   • Hint: one way to solve this is to look at \(\frac{\dot{y}}{\dot{x}}\)
2. Find \(\dot{\theta} = f'(\dot{x}, \dot{y})\)
   • Notice that this derivative depends only on \(\dot{x}\), \(\ddot{x}\), \(\dot{y}\), and \(\ddot{y}\).
   • Notice that \(\omega = \dot{\theta}\), thus given a reference trajectory and its derivatives you can directly find the steering angle to stay on a given trajectory.
   • In other words, one of the controls (steering angle) is now expressed directly as a function of the trajectory.
3. Next solve for \(v = g(\dot{x}, \dot{y}, \ddot{x}, \ddot{y})\)
   • In other words, solve for the velocity control in terms of the trajectory and its derivatives
   • What is \(\dot{x}^2 + \dot{y}^2\) (you can compute this from the kinematic equations)?
4. You have now solved for the controls \(w\) and \(v\) directly in terms of the vehicle's (\(x\), \(y\)) position and derivatives.
5. The robot follows a trajectory when \(x(t) = x_d(t)\) and \(y(t) = y_d(t)\). You can achieve this behavior by
   1. Taking the necessary derivatives of \(x_d(t)\) and $y_d(t)
   2. Substituting these derivatives into your expressions for \(v\) and \(\omega\)
   3. At least in theory, the robot will follow the trajectory. In practice there will be small errors from wheel slipping, dynamics, and control timing.

1. Create a xacro urdf file called twoarm.urdf.xacro for the robot arm.
   • It should read and use lengths \(L_1\) and \(L_2\) and radii \(R_1\) and \(R_2\) from a file config/arm.yaml.
     • Be sure to choose lengths such that \(L_1 \neq L_2\).
     • See Xacro Yaml Support
   • When \(\theta_1 = 0\) and \(\theta_2 = 0\), the robot should be completely horizontal, with the coordinates of the end-effector (i.e., the tip of link 2) relative to the fixed frame being \((L_1 + L_2, 0, 0)\).
   • Each link should be a different color when displayed in rviz.
   • Each link should be visualized as a cylinder, whose height is the length of that link.
   • The collision volume for each link is the same as the visualization and should be specified in the xacro file
   • Each link should have the proper inertia, assuming a mass of 1kg.
     • When \(L\) or \(R\) changes, the inertia should change accordingly
   • There are no joint limits (each joint is continuous).
   • There should be a frame at the location of the robot's end-effector.
   • The arm should be in the \(xy\) plane of your <Fixed Frame>.
     • In other words the robot is rotating in the plane of the "ground" since the z axis points up in rviz.
     • In rviz the x axis is red, y axis is green, and z axis is blue
   • Each joint frame should have the \(z\) axis pointing up (i.e., the \(z\) axis is the axis of rotation) and the \(x\) axis pointing along the length of the arm.
2. A launchfile called arm_basics.launch that starts
   • A robot_state_publisher
   • The joint_state_publisher_gui (optionally, if the argument use_jsp is set to true, by default that argument is false)
   • Rviz, which loads a configuration file that sets up the fixed frame and the rest of the visualization properly
   • Remember to use $(find)$ when referencing files stored in packages.
   • Document how to use your launchfile in the README.md

1. A node called arm_traj that causes the end-effector to track the following trajectory:

   \begin{align} x(t) &= 0.9\cos\frac{2\pi t}{T} \sqrt{(L_1+L_2)^2 - h^2} \\ y(t) &= \frac{2}{3}(L_1 + L_2) \end{align}
   • Let \(h=\frac{2}{3}(L_1 + L_2)\) so that the trajectory is always achievable.
   • The trajectory should result in the end-effector moving back and forth in a straight line with a constant \(y\) value
   • The node should read the parameters \(L_1\), \(L_2\), and \(T\) from the parameter server (but not \(h\), since it is derived).
   • Unlike with the turtle trajectory, here you can specify joint angles directly by publishing sensor_msg/JointState messages on joint_state.
     • Therefore, you only need the inverse kinematics: no derivatives.
   • You can derive the inverse kinematics with some basic geometry, look them up (e.g., in Lynch, Park, Modern Robotics Chapter 6, cite your source), or use tf2 to compute them for you.
   • The locations on the trajectory should be published at a fixed frequency that results in the motion of the arm appearing smooth
   • Each joint state message has a header of type std_msgs.msgs.Header
     • set the stamp member to the current time before you send the message
     • seq is set automatically by the publisher
     • frame_id is used by /tf so it can also be left blank for JointState messages
2. A node called arm_marker that draws markers in rviz at the end-effector position.
   • One cycle should take \(T\) seconds: you should read \(T\) from the parameter server so you use the same value as in your trajectory node
   • The markers should disappear after \(\frac{T}{5}\) seconds
   • The markers should be published at a fixed rate
   • You likely will need to publish these markers at a significantly slower rate than the trajectory updates because they can be slow to process.
   • You can use any shape you want
   • The color and shape should change if the x coordinate is positive or negative
   • Determine the position of the end-effector using a tf listener, and trigger the change based on the result
3. A launchfile called arm_mark.launch that lets you run all the nodes you've created and visualize the results. It should:
   • Use an <include> tag to load arm_basics.launch launchfile, launching rviz and the robot_state_publisher and, either the joint_state_publisher (if use_jsp is true) or your trajectory generation node (if use_jsp is false).
   • The launchfile should load \(L_1\), \(L_2\) and \(T\) into the parameter server from a yaml file called arm.yaml
   • Hint: you may wish to write this launchfile incrementally, adding new <node> tasks for each node you create.
4. An entry in your README.md about what each launchfile and node is called and what it does. Comments in the docstring of each node should describe the ROS api for each node (publishers, subscribers, services, etc.).