You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
923 lines
25 KiB
923 lines
25 KiB
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Stretch PIMU Guide: Interpreting Sensor Data\n",
|
|
"\n",
|
|
"The Stretch Body Pimu package provides a Python API to the Stretch RE1 Power and IMU Board (PIMU). In this guide, we'll look at using the stretch_pimu_jog.py and stretch_pimu_scope.py files to get a idea about the status of base.\n",
|
|
"\n",
|
|
"## Setup\n",
|
|
"\n",
|
|
"Stretch Body is available on PyPi as [hello-robot-stretch-body](https://pypi.org/project/hello-robot-stretch-body/). An accompanying command-line tools package called [hello-robot-stretch-body-tools](https://pypi.org/project/hello-robot-stretch-body-tools/) uses Stretch Body to provide convenient scripts to interact with the robot from the command-line. Both come preinstalled on Stretch RE1, but the following command can be used to ensure you have the latest version.\n",
|
|
"\n",
|
|
"In Jupyter notebooks, code preceded by `!` are run in the command-line instead of the Python interpreter."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"!python -m pip install -q -U hello-robot-stretch-body\n",
|
|
"!python -m pip list | grep hello-robot-stretch-body"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Stretch Body PIMU\n",
|
|
"\n",
|
|
"The Stretch body PIMU jog file is available at stretch_body/tools/bin/stretch_pimu_jog.py, this file provides an easy way to interact with the Base board. This can be helpful to ping any componenet in the base board."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from stretch_body.pimu import Pimu\n",
|
|
"import time"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Now, we'll instantiate an object of Pimu and call it `p`."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p = Pimu()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The cell below displays the docstring for `startup()`. As you can see, the method returns a boolean depending on whether or not the class startup procedure succeeded. Only one instance of the pimu class can exist at once, so if another instance is running elsewhere, the method returns false."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.startup?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Below, we make the call to `startup()`. As part of the startup procedure, this method opens serial ports to the hardware devices, loads the parameters that dictate robot behavior, and launches a few helper threads to poll for status in the background. If it successfully does that then it should return True."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.startup()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"If startup fails unexpectedly, the first thing to check is whether a background process is already running an instance of the robot class. Below we use the `pstree` command to list the tree of background processes, and `grep` to filter for scripts starting with \"stretch_\" (often the \"stretch_xbox_controller_teleop.py\" scripts is running in the background). If we see output below, we should use the `pkill` command to [terminate the conflicting process](https://docs.hello-robot.com/troubleshooting_guide/#rpc-transport-errors-stretch-doesnt-respond-to-commands)."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"!pstree | grep stretch_"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The second method we'll look at is called `stop()`. This method closes connections to all serial ports, releases other resources, and shuts down background threads. We'll wait until the end of the notebook to actually call the method."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.stop?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The next method we'll look at is called `pretty_print()`. This method prints out the entire state of the PIMU in a human readable format."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.pretty_print()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"we'd often like to be able to access the robot's state programmatically. The pull_status() method returns a dictionary with a snapshot of the current state of the PIMU. This can be accessed through the status dictionary and specifying the keyword. Here for example the status of fan is displayed."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.pull_status()\n",
|
|
"p.status['fan_on']"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# User/Utility Functions \n",
|
|
"\n",
|
|
"Next, let's take a look at how we can use the User and Utility Functions in the Pimu class to access PIMU board.\n",
|
|
"\n",
|
|
"These functions are mostly designed to be used during normal operation of stretch body or during development/factory use.\n",
|
|
"\n",
|
|
"Note: All the functions require to be followed by the `push_command()`. The functions add the query to the Python API queue and push command is required to push the queue to hardware drivers."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"IMU RESET: Toggles the reset line to the IMU chip. This potentially could be useful in long running robots is the IMU gets stuck in an error state."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.imu_reset()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"TRIGGER MOTOR SYNC: The motor sync ensures that the motions start at exactly the right time. Otherwise there can be a slight delay (~10ms) between the start of motion for each joint. If motor sync is enabled (via YAML) for the four stepper motors, using `move_to` and `push_command` gets the motion command down to the four stepper motors. But they won't start moving until `trigger_motor_sync` is called."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.trigger_motor_sync()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"SET_FAN_ON and SET_FAN_OFF: This function is used to set the fan in the PIMU board to on or off. We can use the p.pull_status to fetch the status and use p.status['keyword_name'] to display the status. For Fan, we can see if the fan is currently running or not and toggle it to on or off. Stretch body uses Intel-NUC and the fan is useful to prevent issues resulting from overheating."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.pull_status()\n",
|
|
"if p.status['fan_on']:\n",
|
|
" p.set_fan_off()\n",
|
|
"else:\n",
|
|
" p.set_fan_on()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"SET_BUZZER_ON and SET_BUZZER_OFF: Same as in fan, these functions are used to trigger the buzzer on or off. The status values can be used to plot if "
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"if p.status['buzzer_on']:\n",
|
|
" p.set_buzzer_off()\n",
|
|
"else:\n",
|
|
" p.set_buzzer_on()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"BOARD_RESET: board_reset will cause the SAMD microcontroller on the board to reset (as if the board was power cycled). Users generally don't need to call this."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.board_reset()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"CLIFF_EVENT_RESET: The cliff sensors can be configured via YAML to put the robot in pause/runstop mode when they are outside of a threshold value. This will cause the robot to stop when it approaches a cliff. In order to reset this event and allow motion to resume this function must be called. By default we have this functionality turned off at the factory as it isn't well tested."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.cliff_event_reset()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"RUNSTOP_EVENT_RESET: Reset the robot runstop, allowing motion to continue."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.runstop_event_reset()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"RUNSTOP_EVENT_TRIGGER: Trigger the robot runstop, stopping motion."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.runstop_event_trigger()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"TRIGGER_BEEP: You should hear a beep after running this command. This is useful sometimes for generating a sounds wave that can be helped to check if the bump sensors are working as expected. Generate a single short beep."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.trigger_beep()\n",
|
|
"p.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Visualising the status values.\n",
|
|
"\n",
|
|
"It might be useful to actually visualise the continuous status values of the PIMU. This involves pulling the status values from various sensors continuously and plotting it on a graph. This method could be super helpful to understand how changes in the environment change the sensor readings and robot's status."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Let's install the required libraries for visualisation"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from IPython.display import display, clear_output\n",
|
|
"import matplotlib.pyplot as plt"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def get_reading(p, params, functions=None):\n",
|
|
" p.pull_status()\n",
|
|
" value = p.status[params[0]]\n",
|
|
" for key in params[1:]:\n",
|
|
" value = value[key]\n",
|
|
" if functions != None:\n",
|
|
" for function in functions:\n",
|
|
" value = function(value)\n",
|
|
" return value"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def plot_values(num_points, run_time, p, params, y_range=None, functions=None):\n",
|
|
" fig = plt.figure()\n",
|
|
" ax = fig.add_subplot(1, 1, 1) \n",
|
|
" \n",
|
|
" new_val = get_reading(p, params, functions)\n",
|
|
"\n",
|
|
"\n",
|
|
" data = np.full(num_points, new_val)\n",
|
|
"\n",
|
|
" for _ in range(run_time*2):\n",
|
|
" new_val = get_reading(p, params, functions)\n",
|
|
" print(\"new val is \", new_val)\n",
|
|
" \n",
|
|
" data=np.roll(data,-1)\n",
|
|
" data[-1]=new_val\n",
|
|
"\n",
|
|
" ax.cla()\n",
|
|
" ax.plot(data)\n",
|
|
" if y_range == None:\n",
|
|
" ax.set_ylim([new_val-0.1,new_val+0.1])\n",
|
|
" else:\n",
|
|
" ax.set_ylim(y_range)\n",
|
|
" display(fig)\n",
|
|
"\n",
|
|
" clear_output(wait = True)\n",
|
|
" plt.pause(0.5)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def plot_values_4(num_points, run_time, p, params, yrange = None, functions=None):\n",
|
|
" fig = plt.figure()\n",
|
|
" ax = fig.add_subplot(1, 1, 1) \n",
|
|
" \n",
|
|
" new_val = get_reading(p, params, functions)\n",
|
|
" print(new_val)\n",
|
|
"\n",
|
|
" data = np.full([num_points,4], new_val)\n",
|
|
"\n",
|
|
" for _ in range(run_time*2):\n",
|
|
"\n",
|
|
" new_val = get_reading(p, params, functions)\n",
|
|
" \n",
|
|
" print(\"new val is \", new_val)\n",
|
|
" data=np.roll(data,-1,0)\n",
|
|
" data[-1]=new_val\n",
|
|
"\n",
|
|
" ax.cla()\n",
|
|
" ax.plot(data)\n",
|
|
" ax.set_ylim(yrange)\n",
|
|
" display(fig)\n",
|
|
"\n",
|
|
" clear_output(wait = True)\n",
|
|
" plt.pause(0.5)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The value `num_points` signifies the number of values that will be plotted at a point in the line plot. `run_time` signifies the time in seconds till which the visualisation will run. New status value is pulled twise every second. `yrange` signifies the range of y-axis. If this is set to None, the visualisation will set the range as +/- 0.1 of current value. The `params` argument expects a list of the key values to access status. This is used for accessing specific sensor readings within certain devices. The `functions` argument expects a list of functions that could be applied to the values before visualising."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"cliff: Scope base cliff sensors. The cliff sensors are helpful in indicating if the robot is at the edge of a fall or inclined. Try to lighly lift the robot from one side and notice how the sensor value spikes (be careful to not lift is too high as to topple Stretch). The forum post might be helpful when setting up yrange for cliff sensors - https://forum.hello-robot.com/t/working-with-cliff-sensors/88"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-50, 275]\n",
|
|
"\n",
|
|
"plot_values_4(num_points, run_time, p, [\"cliff_range\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"at_cliff: Scope base at_cliff signal"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-0.25,1.25]\n",
|
|
"\n",
|
|
"plot_values_4(num_points, run_time, p, [\"at_cliff\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"voltage: Scope bus voltage (V). This value is helpful for measuring the state of the battery in the robot. If battery is low, this value would be low. It is good practice to not let the voltage value get too low before a recharge."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 30\n",
|
|
"# yrange = [12,15]\n",
|
|
"yrange = None\n",
|
|
"plot_values(num_points, run_time, p, [\"voltage\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"current: Scope bus current (A)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [0,5]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"current\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"temp: Scope base internal temperature (C)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"# yrange = [0,50]\n",
|
|
"yrange= None\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"temp\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"IMU sensor readings: IMU stands for “Inertial Measurement Unit,” and it is used to capture data about the device’s movement. The IMU has accelerometer, gyroscope, magnetometer.\n",
|
|
"\n",
|
|
"The raw data collected from an IMU gives some idea of the world around it, but that information can also be processed for additional insight. Sensor fusion is the (mathematical) art of combining the data from each sensor in an IMU to create a more complete picture of the device’s orientation and heading. For instance, while looking at gyroscope information for rotational motion, you can incorporate an accelerometers sense of gravity to create a reference frame."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The accelerometer measure the velocity and acceleration in the x,y,z direction."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"ax: Scope base accelerometer AX"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-2,2]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"ax\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"ay: Scope base accelerometer AY"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-2,2]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"ay\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"az: Scope base accelerometer AZ"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-11,-8]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"az\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The magnenometer establishes cardinal direction."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"mx: Scope base magnetometer MX"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-20,20]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"mx\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"my: Scope base magnetometer MY"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-20,20]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"my\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"mz: Scope base magnetometer MZ"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-20,20]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"mz\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
" The gyrocope measures the rotation and rotational rate."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"gx: Scope base gyro GX"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"# yrange = [-2,2]\n",
|
|
"yrange=None\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"gx\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"gy: Scope base gyro GY"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"# yrange = [-2,2]\n",
|
|
"yrange=None\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"gy\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"gz: Scope base gyro GZ"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"# yrange = [-2,2]\n",
|
|
"yrange=None\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"gz\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"roll: Scope base imu Roll"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"# yrange = [-2,2]\n",
|
|
"yrange=None\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"roll\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"pitch: Scope base imu Pitch"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange=[-20,20]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"pitch\"], yrange, [np.rad2deg])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"heading: Scope base imu Heading"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-10,370]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"heading\"], yrange,[np.rad2deg])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"bump: Scope base imu bump level. The bump value is useful for indicating collision of the robot. The value spikes up when there is any collision. You can test this by lightly hitting the base (be careful to not hit too hard to damage the body)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 20\n",
|
|
"run_time = 10\n",
|
|
"yrange = [-1,15]\n",
|
|
"\n",
|
|
"plot_values(num_points, run_time, p, [\"imu\",\"bump\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Examples of using the functions"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"When testing out the bump function, the trigger beep could be used to generate sound vibrations that trigger the imu bump sensors. The proper working of this is a good test to see if the sensor is as sensitive as expected."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"num_points = 30\n",
|
|
"run_time=30\n",
|
|
"iterations=2\n",
|
|
"\n",
|
|
"for _ in range(iterations):\n",
|
|
" p.trigger_beep()\n",
|
|
" p.push_command()\n",
|
|
" plot_values(num_points, run_time, p, [\"imu\",\"bump\"], yrange)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Note: Further examples to be added."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Wrapping Up\n",
|
|
"\n",
|
|
"In this notebook, we've covered:\n",
|
|
"\n",
|
|
" * User/Utility functions in PIMU\n",
|
|
" * Visualisation of sensor readings from the PIMU\n",
|
|
" * Some examples of using the above\n",
|
|
"\n",
|
|
"For more information on Stretch Body API, take a look at the [API Documentation](https://docs.hello-robot.com/stretch_body_guide/). To reports bugs or contribute to the library, visit the [Stretch Body Github repo](https://github.com/hello-robot/stretch_body/) where development on the library happens. Also, feel free to join our community on the [forum](https://forum.hello-robot.com/) and learn about research/projects happening with Stretch."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"p.stop()"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "Python 2",
|
|
"language": "python",
|
|
"name": "python2"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 2
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython2",
|
|
"version": "2.7.17"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|