UP | HOME

Homework 3

Logistics

  1. Create a git repository using the link provided in Canvas.
  2. Your homework will be submitted via this git repository.
  3. I will grade whatever is on the main branch when I clone the repository.
  4. Guidelines provides important information about what you should turn-in, including information about your README.md file.

  5. You will create two separate ROS packages for this assignment but store them in the same git repository. T 

    my_git_repository/diff_drive/{package.xml, CMakeLists.txt, README.md, etc}
my_git_repository/arm_move/{package.xml, CMakeLists.txt, README.md, etc}
    • Each package should have it's own README.md.

Robots

  1. This assignment will use the pincherX 100 robot.
  2. If you are not an MSR student, follow the setup guide: PincherX 100

Part I: Gazebo Differential Drive Challenge

The challenge is to create a differential-drive robot that can be simulated in Gazebo and controlled by ROS.

  • The robot will be able to flip over and continue driving
  • The robot will move through a world filled with Jersey Barriers and trash.

Your submission should be a subdirectory with

  • package.xml
  • CMakeLists.txt
  • /config (.yaml files)
  • /urdf (.xacro and .gazebo files)
  • /launch (.launch files)
  • /worlds (.world files)
  • /nodes (Your ROS nodes.)

There is a hints section at the bottom of each section.

Creating the World

  1. Use the Gazebo GUI to create your world. Place an asphalt plane over the ground plane. Then add some Jersey Barriers (at least 2), A few cardboard boxes (at least 3) and something else you find interesting. Save your world as ddrive.world in the world/ directory of your project
  2. Write a launchfile called ddrive.launch. For now, it should launch gazebo and load the world you just created.

The Xacro URDF

Robot Description

  1. The robot has a body consisting of a rectangular prism. It has two cylindrical wheels on the front and a caster at the rear.
    • There should be a caster on the top and bottom of the robot so that it is symmetrical about its horizontal plane
    • Each caster link should be modified to have no friction in gazebo so that it can slide freely.
    • Feel free to set the masses and dimensions of the robot to suit your needs.
  2. Create a ddrive.urdf.xacro file to describe the differential-drive robot.
    • The dimensional parameters of the robot should be stored in a .yaml file and referenced in the xacro file
    • Each link you add must have the proper collision bounding box and inertia.
    • Place gazebo specific tags in a separate ddrive.gazebo file that is included in your main urdf xacro.
  3. Create a launchfile called ddrive_rviz.launch that launches the urdf in RViz, along with a robot_state_publisher and optionally a joint_state_publisher_gui. This launchfile will be useful for debugging.
  4. Modify ddrive.launch so that it spawns your URDF model in the gazebo world.
    • Use a launchfile argument to start gazebo with the simulation paused. You will need to remember to unpause when you actually want the simulation to run.
  5. Modify ddrive.launch so that it loads the urdf into the /robot_description parameter.

Gazebo Extensions

  1. Add gazebo materials to each of your links so they appear as an appropriate color.

  2. Add the "Differential Drive" plugin to your robot.

    • You may adapt the following code (which I adapted from the Gazebo Plugins in Ros Tutorial 

      <gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>na</rosDebugLevel>
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<leftJoint>left_wheel</leftJoint>
<rightJoint>right_wheel</rightJoint>
<wheelSeparation>${body_width+wheel_width}</wheelSeparation>
<wheelDiameter>${wheel_radius}</wheelDiameter>
<wheelTorque>20</wheelTorque>
<commandTopic>diff/cmd_vel</commandTopic>
<odometryTopic>diff/odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishOdomTF>true</publishOdomTF>
<publishWheelJointState>true</publishWheelJointState>
<wheelAcceleration>false</wheelAcceleration>
<odometrySource>world</odometrySource>
<publishTf>1</publishTf>
</plugin>
</gazebo>
    • The above code assumes your wheel joints are called left_wheel and right_wheel and also refers to some xacro parameters I defined for the dimensions of the robot. You may need to modify it to match your urdf.xacro file.
    • Create a new rviz configuration using the odom frame as the fixed frame. The odom frame is published by the differential drive gazebo plugin
    • Launch rviz with this configuration so that you can visualize where the robot thinks it is, according to it's wheel odometry.

Control

  1. Write a node called flip that makes the robot move back and forth along a line by flipping over itself rather than turning. (i.e., both wheels should move with the same velocity at all times). You will need to use the dynamics of the robot to accomplish this task. You may find that the teleop_twist_keyboard package is useful for letting you get a feel for the dynamics.
    • Precise control is great but not necessary, just have fun doing some flips.

Hints

  1. If your model is loaded into the /robot_description parameter, and gazebo is running: use rosrun gazebo_ros spawn_model -urdf -param robot_description -model ddrive -b
    • The -b will cause gazebo to delete the model when you kill the node
    • The -param loads the urdf from the parameter server. You can use -file to load from a urdf file (converted using xacro manually)
    • Links will not be exported to the sdf unless they have visual and inertial elements.
    • I suggest pausing the simulation when you spawn the robot.
      • If you are not careful about where you spawn the robot or having a collision element (so it doesn't fall through the ground), it can fly off into space.
    • You can run the spawn_model node from a launchfile for easier use
  2. For this exercise, you may wish to use xacro macros to create the visual, collision, and inertial elements from a single set of parameters specifying the geometry and orientation of the link.
    • For example, in this case the visual and collision elements can be identical
    • Macros can be nested. So build them up slowly and view the results incrementally by outputting the urdf file from the xacro file.
  3. For your links to show up with the proper colors in gazebo, you need to assign them a gazebo material, as per the Using URDF in Gazebo tutorial.
    • The URDF material tag does not get translated so your links will just show up as gray if you don't include this separately.
  4. The view menu in gazebo can let you visualize the inertial properties See in the "Checking in Gazebo section" of http://gazebosim.org/tutorials?tut=inertia&cat=build_robot
  5. If gazebo is having trouble opening, it may not have exited cleanly:
    • ps aux | grep gazebo will show gazebo processes to kill
    • rosnode kill will show ros nodes to kill
    • ps aux | grep ros will show ros processes to kill
    • You can try killall gzserver to kill the gazebo server
    • You can try killall gzclient to kill the gazebo client
  6. It is helpful to not need to restart gazebo every time you want to test your robot.
    • Feel free to arrange your launchfiles such that starting gazebo is optional and in a way that makes it easy to spawn/remove your model
    • You can also call gazebo services from the command-line or use rosrun to help during debugging
  7. The simulation can be rather finicky when it comes to ground contact and friction with the wheels. If you are having trouble with the robot sliding oddly or not moving when you spin the wheels:
    1. Make sure that the bottom of the caster and the bottom of the wheels are at the exact same height
    2. Make sure that the dimensions and mass of the vehicle are reasonable in relation to each other
    3. Make sure the wheels have high friction coefficients (.e.g, <mu1>999</mu1> and <mu2>999</mu2> i

    4. You can adjust some collision properties in the gazebo part of the urdf, which may help: 

      <gazebo reference="wheel_name_here">
<mu1>1</mu1>
<mu2>1</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
</gazebo>
      • Depending on your vehicle inertias, you may need to adjust these parameters
      • Read more about the parameters here: Gazebo Elements For Links
  8. If your robot is drifting a little when not commanded with a velocity that is okay. You can usually correct for this by always explicitly sending a /cmd_vel command of 0.

Part II: Motion Planning

  1. You will do this final task on the real robot and in gazebo.
  2. The overall goal is to train a robot to follow a series of waypoints to grab an object
  3. Copy the launchfile used by the px100 gazebo program to arm_box.launch and modify it so that it loads a custom world containing a RealSense Box. The box is 14cm x 9cm x 5cm and weights 100g
  4. In this project you will use the MoveIt Python API to create a planning scene and perform some path planning tasks
    • The robot arm sits on top of a table. The RealSense Box also sits on top of the table and is an obstacle
  5. Create a node called mover that implements the following:
    1. Upon startup, adds a collision object representing the table to the planning scene.
    2. The node loads a list of waypoints from the parameter server
      • If waypoints do not yet exist, the list is empty
    3. A reset service that
      • Takes a position and orientation of the realsense box and creates/moves the Realsense Box to the appropriate location
      • Moves the robot to its pre-defined 'Home' position without hitting the box ('Home' is pre-defined by the SDK)
      • Reset takes a parameter that determines if the waypoints are cleared or not.
    4. A step service that moves the robot to a user-specified position and also a gripper state (open or closed)
      • step returns the MoveItErrorCode to indicate success or failure of the plan
      • If the motion plan succeeds, step adds the position to the list of waypoints, storing it in the parameter server, and executes the plan
      • If the motion plan fails, it is okay, use the MoveItErrorCode to interpret why
    5. A follow service that has the robot move to each waypoint, in sequence
      • Follow should contain an option that determines if the cycle should be repeated or run just once.
  6. Add another small light object to the (real/simulated) world. Use the step and follow services to generate a sequence of waypoints that allows the robot to pick up the object from a fixed location and drop it off elsewhere.
    • When you get a working sequence, save the corresponding ROS parameters to a yaml file and include it in your repository
    • Your launchfile should load the waypoints from the yaml file
  7. Write a launchfile called arm.launch
    • The launchfile should setup the robot or simulation (depending on the value of an argument called use_sim) to be controlled from the mover node
    • Include an option to run the MoveIt fake_node instead of gazebo, when use_sim is true.
    • The mover node should start and be loaded with an initial waypoint sequence
    • RViz should be loaded. Rviz should show the robot and the planning scene
  8. Write a launchfile called arm.test for your integration tests
    • This should use fake moveit and also start your mover node.
    • It should call reset service
    • It should then call the step service in such a way that the robot will crash into the floor
      • Verify that the step service returns an error code
    • It should also call the step service in such a way that it succeeds.
      • Verify the success
    • This test launchfile should be added to catkin so that it runs with catkin_make run tests
  9. Include a link to a video in your README.md of the robot in gazebo and in the real world accomplishing its task

Hints

  1. It is often easier to launch gazebo and keep it running. Then just kill and manually restart your node
  2. You may wish to do the bulk of your testing with the fake MoveIt node prior to involving the simulation or the real robot.
    • With the interactive moveit view you can interactively determine lots of information about the robot such as
      • It's planning groups,
      • It's built-in positions
  3. You may wish to run through some tutorials prior to attempting the assignment:
  4. Remember, this robot has fewer than 6-dof, so many 6-DOF poses of the end effector are invalid.
  5. You can set_position_target to set position without respect to pose and set_orientation_target or set_rpy_target to set one without regard for the other. You can also plan to joint positions.
  6. get_current_pose() is also helpful for debugging to see the exact current location of the end effector
  7. You can try out most things in the RViz GUI prior to coding them
  8. There are two tricky parts to PlanningSceneInterface that you must work around
    1. After creating the PlanningSceneInterface, it's publishers may not have necessarily finished connecting to their respective topics
      • This means that you can potentially add an object prior to the PlanningSceneInterface being ready, in which case the object will not get added
    2. Your node could exit after modifying the planning scene but before the message is published.
    3. To solve these issues, you should, after every action on a planning scene, wait (with a timeout) to see if your changes actually took effect
      • Sample code for doing this is provided in the tutorial
      • Depending on the context, the action you take after a timeout failure is different
      • For example, if you just started the planning scene, you may wish to retry the addition a few times upon timing out
  9. All parameters/names used by the interbotix arm are stored under the /px100 namespace. This means that you will need to specify/remap names throughout this assignment so that they use the appropriate values in the /px100 namespace
    • In particular, when launching the mover node you should
      1. remap /attached_collision_object to /px100/attached_collision_object
      2. remap /collision_object to /px100/collision_object
      3. Launch your node in the px100 namespace
  10. It is up to you what rotational representation (e.g., quaternions or euler angles) to use for your services
  11. If the gazebo simulation drops the object, you may wish to edit custom_ws/interbotix_ros_manipulators/interbotix_xsarm_gazebo/config/interbotix_texture.gazebo to increase the coefficient of friction on the grippers.

Resources

  1. The MoveIt Notes
  2. The Gazebo notes
  3. See Test A ROS Node

1 The moveit tutorials https://ros-planning.github.io/moveit_tutorials/

  • It may help to go through some tutorials prior to starting
  • MoveitQuickstart in Rviz, Move Group Python Interface are the two most important
  1. Full Python API documentation

Author: Matthew Elwin.