This commit is contained in:
jamie wilbraham
Родитель
da123b4122
Коммит
e401210b3d
|
|
@ -23,9 +23,14 @@ bld/
|
|||
[Bb]in/
|
||||
[Oo]bj/
|
||||
[Ll]og/
|
||||
build/
|
||||
devel/
|
||||
logs/
|
||||
.catkin_tools/
|
||||
|
||||
# Visual Studio 2015/2017 cache/options directory
|
||||
.vs/
|
||||
.vscode
|
||||
# Uncomment if you have tasks that create the project's static files in wwwroot
|
||||
#wwwroot/
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,15 @@
|
|||
[submodule "submodules/intera_sdk"]
|
||||
path = src/submodules/intera_sdk
|
||||
url = https://github.com/RethinkRobotics/intera_sdk
|
||||
[submodule "submodules/sawyer_simulator"]
|
||||
path = src/submodules/sawyer_simulator
|
||||
url = https://github.com/RethinkRobotics/sawyer_simulator.git
|
||||
[submodule "submodules/intera_common"]
|
||||
path = src/submodules/intera_common
|
||||
url = https://github.com/RethinkRobotics/intera_common
|
||||
[submodule "submodules/sawyer_moveit"]
|
||||
path = src/submodules/sawyer_moveit
|
||||
url = https://github.com/RethinkRobotics/sawyer_moveit
|
||||
[submodule "submodules/sawyer_robot"]
|
||||
path = src/submodules/sawyer_robot
|
||||
url = https://github.com/RethinkRobotics/sawyer_robot.git
|
||||
429
README.md
429
README.md
|
|
@ -1,3 +1,430 @@
|
|||
# AI Robot Challenge
|
||||
|
||||
The future is the fusion of AI and robotics to enable intelligent, collaborative, assistive, and social robots that augment human ingenuity. If you want to take the AI Robot Challenge but are new to robotics, each day we will present an Intro to Robotics session. In this session, you will learn about the heart of robotics programming using the Robot Operating system (ROS) with Python and how to use Gazebo, the robot simulator. You will also learn how to deploy your code to a real industrial robot. This lab will give you the confidence to start your journey with intelligent collaborative robotics.
|
||||
|
||||
# Introduction to Robotics
|
||||
|
||||
## Summary of technologies used
|
||||
|
||||
### **ROS**
|
||||
|
||||
ROS (Robot Operating System) is robotics middleware licensed under an open source, BSD license. Althought ROS is not an Operative System, it provides libraries, hardware abstraction, device drivers, visualizers, message-passing, package management, and other tools to help software developers create robot applications.
|
||||
|
||||
Last Release: ROS Melodic Morenia. Supported on Ubuntu Artful and Bionic, along with Debian Stretch.
|
||||
Supports collaboration throught Repositories.
|
||||
ROS is a distributed framework of processes (aka Nodes) that enables executables to be individually added, which makes the framework very modular. These processes can be grouped into Packages and Stacks, which can be easily shared and distributed.
|
||||
|
||||
Althought there are some Robotics Kits available in the market, ROS is quickly becoming the new standard for both industrial and research robotics as it integrates both hardware and software in their solution for industrial applications.
|
||||
|
||||
**Languages:** Python 2.7
|
||||
|
||||
**System Requirements:** Ubuntu 16.04
|
||||
|
||||
**Other Supported Technologies (reference only)**
|
||||
C++ and Lisp
|
||||
|
||||
Supports Unix-based systems, primarly Ubuntu and Mac OS X systems, but the community has been adding support to Fedora, Gentoo, Arch Linux and other Linux platforms.
|
||||
|
||||
|
||||
### **Gazebo**
|
||||
|
||||
Gazebo is a 3D robot simulation tool that seamlessly integrates with ROS, which allows to run the Gazebo server in a ROS environment. Gazebo allows to build 3D scenarios on your computer with robots, using obstacles and other objects. This allows to test robots in complex or dangerous scenarios without any harm to the real robot. Most of the time it is faster and cost effective to simply run a simulator instead of starting the whole scenario on a real robot. It uses a physics engines for realistic movements called ODE (Open Dynamics Engine).
|
||||
|
||||
Gazebo has two main components: the server which acts like a back-end to compute all the logic for your testing scenarios and a client which works as a graphical front-end. This is very useful as sometimes you might only want to execute the tests without the UI in order to speed up the execution process.
|
||||
Gazebo was used to simulate the atlas robot for the Virtual Robotics Challenge (the precursor to the DARPA robotics challenge), which required to build a software simulation of humanoid rescue work.
|
||||
|
||||
**Languages:** C++ API
|
||||
|
||||
**System Requirements:** Linux
|
||||
|
||||
|
||||
### **RViz**
|
||||
|
||||
RViz is an Open Source 3D visualizer for the Robot Operating System (ROS) framework.
|
||||
Uses sensors data and custom visualization markers to develop robot capabilities in a 3d environment.
|
||||
Features:
|
||||
- Motion planning: the process of breaking down a desired movement task into discrete motions that satisfy movement constraints and possibly optimize some aspect of the movement.
|
||||
- Object detection: vizualization and recognition of objects using the camera, for example recognizing cubes of different colors.
|
||||
- Calibration: geometric camera calibration is used to estimate parameters internal to the camera that affect the image processing.
|
||||
- Debugging
|
||||
- RViz visualization widget
|
||||
- 3D stereo rendering, using 2 connected cameras to display 3D images, it displays a different view to each eye so that the scene appears to have depth.
|
||||
RViz provides a CLI tool that lets you execute python or c++ scripts with controls.
|
||||
|
||||
### **Sawyer**
|
||||
|
||||
Sawyer is an integrated collaborative robot (aka cobot) solution designed with embedded vision, smart swappable grippers, and high resolution force control. The robot purpose is to automate specific industrial repetive tasks, it comes with an arm that has a gripper which can be easily replaced by one of the available options from the ClickSmart Gripper Kit.
|
||||
Features:
|
||||
- Sawyer comes with highly sensitive torque sensors embedded into every joint, this allows you to control force where delicate part insertion is critical, or use force if needed. It can maneuver into tight spaces and it has a long reach of 1260 mm (50 inches) max.
|
||||
- Comes with an embedded vision system used for the robot positioning, it also allows external vision systems like cameras.
|
||||
- Fast to deploy as many pieces are plug&play with integrated sensors.
|
||||
|
||||
### **MoveIt!**
|
||||
|
||||
MoveIt is software for motion and path planning. Users can access actions and services using: C++, Python, Through a GUI.
|
||||
Features:
|
||||
- Manipulation
|
||||
- Motion planning: the process of breaking down a desired movement task into discrete motions that satisfy movement constraints and possibly optimize some aspect of the movement.
|
||||
- 3D perception: allows the robot to extract 3D information from the world and their own movements so that they accomplish navigation and manipulation tasks.
|
||||
- Kinematics: geometry for moving the arms.
|
||||
- Control and navigation: underneath it uses OMPL (Open Motion Planning Library) and requires a controller to send messages to the hardware. MoveIt provides a Fake Controller to interact with the hardware using ROS messages but you can replace the fake robot controller in MoveIt with your own plugin which controls the robot if needed.
|
||||
The planning scene feature allows to monitor the state, sensor and world geometry information.
|
||||
|
||||
# Getting started
|
||||
|
||||
Microsoft Bot Framework and Cognitive Services provide a platform to develop intelligent bots. Bot Framework allows us to develop bots in different languages and by adding congitive services to the bot, we are able to make our bot smart and have capabilities like language understanding, image recognition, text recognition, translation and more. In this lab we will create a simple bot and well make this bot to communicate with a physical robot using natural language and Computer Vision for image recognition.
|
||||
|
||||
## Setup your environment
|
||||
|
||||
### Setup your Azure subscription
|
||||
|
||||
This lab **requires** an Azure subscription. If you delete the resources at the end of the session, total charges will be less than $1 so we strongly recommend using an existing subscription if available.
|
||||
|
||||
If you need a new Azure subscription, then there are a couple of options to get a free subscription:
|
||||
|
||||
1. The easiest way to sign up for an Azure subscription is with VS Dev Essentials and a personal Microsoft account (like @outlook.com). This does require a credit card; however, there is a spending limit in the subscription so it won't be charged unless you explicitly remove the limit.
|
||||
* Open Microsoft Edge and go to the [Microsoft VS Dev Essentials site](https://visualstudio.microsoft.com/dev-essentials/).
|
||||
* Click **Join or access now**.
|
||||
* Sign in using your personal Microsoft account.
|
||||
* If prompted, click Confirm to agree to the terms and conditions.
|
||||
* Find the Azure tile and click the **Activate** link.
|
||||
1. Alternatively, if the above isn't suitable, you can sign up for a free Azure trial.
|
||||
* Open Microsoft Edge and go to the [free Azure trial page](https://azure.microsoft.com/en-us/free/).
|
||||
* Click **Start free**.
|
||||
* Sign in using your personal Microsoft account.
|
||||
1. Complete the Azure sign up steps and wait for the subscription to be provisioned. This usually only takes a couple of minutes.
|
||||
|
||||
|
||||
### Get ubuntu 16.04 image
|
||||
|
||||
1. [Download](http://releases.ubuntu.com/16.04/) an Ubuntu 16.04 image.
|
||||
1. Install the image in a VM.
|
||||
> [!NOTE] You can use any virtualization software to run the image
|
||||
1. Make sure to allocate at least 8GB of RAM.
|
||||
|
||||
### Run installation script on VM
|
||||
|
||||
1. Clone this repo into your **Home** folder by runnning the following command `git clone https://github.com/Microsoft/AI-Robot-Challenge-Lab.git` from a **Terminal** console.
|
||||
1. Navigate to `~/AI-Robot-Challenge-Lab/setup` in a Terminal window.
|
||||
1. Run the following command: `chmod +x robot-challenge-setup.sh`.
|
||||
1. Run the shell script with the following command `./robot-challenge-setup.sh`.
|
||||
|
||||
### Setup and launch the simulator
|
||||
|
||||
1. Open a Terminal and navigate to `~/AI-Robot-Challenge-Lab/src`.
|
||||
2. Initialize git submodules:
|
||||
* `git submodule init`
|
||||
* `git submodule update`
|
||||
3. Move to the parent directory:
|
||||
* `cd ..`
|
||||
4. Run the following command:
|
||||
* `rosdep install --from-paths src --ignore-src -r -y`
|
||||
5. Build the code:
|
||||
* `catkin build`
|
||||
6. Run the following commands to launch the simulator:
|
||||
```
|
||||
cd $HOME/ros_ws && ./intera.sh sim
|
||||
cd ~/AI-Robot-Challenge-Lab && source devel/setup.bash && roslaunch sorting_demo sorting_demo.launch
|
||||
```
|
||||
|
||||
### Setup Language Understanding
|
||||
|
||||
Language Understanding (LUIS) allows your bot to understand what a person wants in their own words. LUIS uses machine learning to allow developers to build applications that can receive user input in natural language and extract meaning from it.
|
||||
|
||||
### Create a LUIS subscription
|
||||
|
||||
While LUIS has a standalone portal for building the model, it uses Azure for subscription management.
|
||||
|
||||
Create the LUIS resource in Azure:
|
||||
|
||||
1. Return to the Azure Portal (++portal.azure.com++).
|
||||
1. Click **Create Resource [+]** from the left menu and search for **Language Understanding**.
|
||||
1. **Select** the first result and then click the **Create** button.
|
||||
1. Provide the required information:
|
||||
* App name: `robotics-luis-<your_initials>`.
|
||||
* Location: `West US`.
|
||||
* Pricing tier: `F0 (5 Calls per second, 10K Calls per month)`.
|
||||
* Create a new resource group with the name: `robotics-lab-<your initials>`.
|
||||
* **Confirm** that you read and understood the terms by **checking** the box.
|
||||
1. Click **Create**. This step might take a few seconds.
|
||||
1. Once the deployment is complete, you will see a **Deployment succeeded** notification.
|
||||
1. Go to **All Resources** in the left pane and **search** for the new resource (`robotics-luis-<your initials>`).
|
||||
1. **Click** on the resource.
|
||||
1. Go to the **Keys** page.
|
||||
1. Copy the **Key 1** value into **Notepad**.
|
||||
|
||||
> [!NOTE] We'll need this key later on.
|
||||
|
||||
### Create a new LUIS App
|
||||
|
||||
Before calling LUIS, we need to train it with the kinds of phrases we expect our users to use.
|
||||
|
||||
1. Login to the [LUIS portal](www.luis.ai).
|
||||
|
||||
> [!NOTE] Use the same credentials as you used for logging into Azure.
|
||||
1. **Scroll down** to the bottom of the welcome page.
|
||||
1. Click **Create new app**.
|
||||
1. Select **United States** from the country list.
|
||||
1. Check the **I agree** checkbox.
|
||||
1. Click the **Continue** button.
|
||||
1. From `My Apps`, click **Import new app**.
|
||||
1. **Select** the base model from `~/AI-Robot-Challenge-Lab/resources/robotics-bot-luis-app.json`.
|
||||
1. Click on the **Done** button.
|
||||
1. **Wait** for the import to complete.
|
||||
1. Click on the **Train** button and wait for it to finish.
|
||||
1. Click the **Test** button to open the test panel.
|
||||
1. **Type** `move arm` and press enter.
|
||||
|
||||
> [!NOTE] It should return the `MoveArm` intent.
|
||||
|
||||
# Bringing Your Robot to Life
|
||||
|
||||
We created a basic bot using the SDK V4, we'll run it locally using the Bot Framework Emulator and extend its functionality by using Language Understanding to request different operations to the robot. The code base includes a method to make the physical robot move/wave the arm, this method invokes a script that uses the **Intera SDK** to send commands to the robot.
|
||||
|
||||
## Implement your Bot
|
||||
|
||||
### Add support for Language Understanding
|
||||
|
||||
Let's add language understanding support to the bot.
|
||||
|
||||
1. Open **Visual Studio Code**.
|
||||
1. Click on **Open Folder** and select the `~/AI-Robot-Challenge-Lab/src/chatbot` folder that you extracted earlier.
|
||||
1. Click on `talk-to-my-robot.py` to open the bot python script.
|
||||
1. If prompted to install the Python Extension select Install, once installed, select **Reload** to activate the extension.
|
||||
1. Click on **View -> Command Palette** from the top menu and type `Python:Select Interpreter`. You should see python 3.6 in the options, select this version.
|
||||
1. Go to the `BotRequestHandler` class.
|
||||
1. Modify the method `handle_message`, replace this line
|
||||
```python
|
||||
luis_result = LuisResponse('None')
|
||||
```
|
||||
|
||||
with
|
||||
```python
|
||||
luis_result = LuisApiService.post_utterance(activity.text)
|
||||
```
|
||||
> [!NOTE] This method is the entry point of the bot messages, here we can see how we get the incoming request, then send it to LUIS and use the intent result to trigger specific operations, in this case it already provides support to handle the **MoveArm** intent.
|
||||
1. Go to the `LuisApiService` class.
|
||||
1. Modify the method `post_utterance`:
|
||||
* Add the following code after the line `Request headers and parameters`
|
||||
```python
|
||||
headers = {'Ocp-Apim-Subscription-Key': LUIS_SUBSCRIPTION_KEY}
|
||||
params = {
|
||||
# Query parameter
|
||||
'q': message,
|
||||
# Optional request parameters, set to default values
|
||||
'timezoneOffset': '0',
|
||||
'verbose': 'false',
|
||||
'spellCheck': 'false',
|
||||
'staging': 'false',
|
||||
}
|
||||
```
|
||||
* Replace the line `return None` with the following code snippet
|
||||
```python
|
||||
r = requests.get('https://westus.api.cognitive.microsoft.com/luis/v2.0/apps/%s' % LUIS_APP_ID, headers=headers, params=params)
|
||||
topScoreIntent = r.json()['topScoringIntent']
|
||||
entities = r.json()['entities']
|
||||
intent = topScoreIntent['intent'] if topScoreIntent['score'] > 0.5 else 'None'
|
||||
entity = entities[0] if len(entities) > 0 else None
|
||||
|
||||
return LuisResponse(intent, entity['entity'], entity['type']) if entity else LuisResponse(intent)
|
||||
```
|
||||
> [!ALERT] Check your indentation to avoid python compilation errors.
|
||||
1. Save the **talk-to-my-robot.py** file.
|
||||
|
||||
|
||||
### Test the 'move your arm' command
|
||||
|
||||
The bot emulator provides a convenient way to interact and debug your bot locally. Let's use the emulator to send requests to our bot:
|
||||
1. Select **Debug -> Start Without Debugging** then click **Python** to execute the bot script in **VSCode**.
|
||||
1. Open the **Bot Framework Emulator** app.
|
||||
1. Click **Open Bot** and select the file `SawyerBot.bot` from your **chatbot** directory.
|
||||
|
||||
> [!NOTE] Previously we had to provide the bot endpoint to the emulator but now it can read all the configuration from a file.
|
||||
1. **Type** `move your arm` and press enter.
|
||||
1. Return to **Gazebo** and wait for the simulator to move the arm.
|
||||
1. **Stop** debugging by clicking the stop button in **VSCode** toolbar.
|
||||
|
||||
|
||||
### Make the grippers open & close
|
||||
|
||||
1. Go to the `BotRequestHandler` class.
|
||||
1. Modify the method `handle_message`, after the **if** statement
|
||||
```python
|
||||
if luis_result.intent == 'MoveArm':
|
||||
BotCommandHandler.move_arm()
|
||||
```
|
||||
Add the following code snippet
|
||||
```python
|
||||
elif luis_result.intent == 'MoveGrippers':
|
||||
BotCommandHandler.move_grippers(luis_result.entity_value)
|
||||
```
|
||||
1. Go to the `BotCommandHandler` class.
|
||||
1. Replace the method `move_grippers` content with the following code snippet:
|
||||
```python
|
||||
print(f'{action} grippers... wait a few seconds')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-move-grippers.py -a {}".format(action)
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
|
||||
print('done moving grippers . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + output.decode("utf-8"))
|
||||
```
|
||||
1. Save the **talk-to-my-robot.py** file.
|
||||
|
||||
### Test the grippers move
|
||||
|
||||
1. Select **Debug -> Start Without Debugging** then click **Python** to execute the bot script in **VSCode**.
|
||||
1. Go back to the **Bot Framework Emulator** app.
|
||||
1. Click **Start Over** to start a new conversation.
|
||||
1. **Type** `close grippers` and press enter.
|
||||
1. Return to **Gazebo** and wait for the simulator to move the grippers.
|
||||
1. Go back to the **Bot Framework Emulator** app.
|
||||
1. **Type** `open grippers` and press enter.
|
||||
1. Return to **Gazebo** and wait for the simulator to move the grippers.
|
||||
1. **Stop** debugging by clicking the stop button in **VSCode** toolbar.
|
||||
|
||||
### Show stats
|
||||
|
||||
1. Go to the `BotRequestHandler` class.
|
||||
1. Modify the method `handle_message`, after the **if** statement
|
||||
```python
|
||||
if luis_result.intent == 'MoveArm':
|
||||
BotCommandHandler.move_arm()
|
||||
```
|
||||
Add the following code snippet
|
||||
```python
|
||||
elif luis_result.intent == 'ShowStats':
|
||||
stats = BotCommandHandler.show_stats()
|
||||
response = await BotRequestHandler.create_reply_activity(activity, stats)
|
||||
await context.send_activity(response)
|
||||
```
|
||||
1. Go to the `BotCommandHandler` class.
|
||||
1. Replace the method `show_stats` content with the following code snippet:
|
||||
```python
|
||||
print('Showing stats... do something')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-stats-node.py"
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
result = output.decode("utf-8")
|
||||
|
||||
print('done getting state . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + result + '\n')
|
||||
return result
|
||||
```
|
||||
1. Save the **talk-to-my-robot.py** file.
|
||||
|
||||
### Test the show statistics
|
||||
|
||||
1. Select **Debug -> Start Without Debugging** then click **Python** to execute the bot script in **VSCode**.
|
||||
1. Return to the **Bot Framework Emulator** app.
|
||||
1. Click **Start Over** to start a new conversation.
|
||||
1. **Type** `show stats` and press enter.
|
||||
1. Wait a few seconds and wait for a response from your bot, it will display the stats in the emulator.
|
||||
1. **Stop** debugging by clicking the stop button in **VSCode** toolbar.
|
||||
|
||||
|
||||
## Making Your Robot Intelligent with Microsoft AI
|
||||
|
||||
We will use Computer Vision to extract information from an image and the Intera SDK to send commands to our robot. For this scenario we'll extract the dominant color from an image and the robot will pickup a cube of the color specified.
|
||||
|
||||
### Create a Computer Vision subscription
|
||||
|
||||
The Computer Vision API requires a subscription key from the Azure portal. This key needs to be either passed through a query string parameter or specified in the request header.
|
||||
|
||||
1. Return to the [Azure Portal](portal.azure.com).
|
||||
1. Click **Create Resource [+]** from the left menu and search for **Computer Vision**.
|
||||
1. **Select** the first result and then click the **Create** button.
|
||||
1. Provide the required information:
|
||||
* Name: `robotics-computer-vision-<your initials>`.
|
||||
* Select your preferred subscription.
|
||||
* Select the location: `West US`.
|
||||
* Select the the Pricing tier: `F0 (20 Calls per minute, 5k Calls per month)`.
|
||||
* Select the previously created resource group: `robotics-lab-<your initials>`.
|
||||
1. Click **Create** to create the resource and deploy it. This step might take a few moments.
|
||||
1. Once the deployment is complete, you will see a **Deployment succeeded** notification.
|
||||
1. Go to **All Resources** in the left pane and **search** for the new resource (`robotics-computer-vision-<your initials>`).
|
||||
1. **Click** on the resource.
|
||||
1. Go to the **Keys** page.
|
||||
1. Copy the **Key 1** value into **Notepad**.
|
||||
|
||||
> [!NOTE] We'll need this key in the next step.
|
||||
|
||||
### Add Computer Vision key to your script
|
||||
|
||||
1. Return to **Visual Studio Code**.
|
||||
1. Open the **talk-to-my-robot.py** file.
|
||||
1. Replace the `COMPUTER_VISION_SUBSCRIPTION_KEY` with the subscription key previously obtained.
|
||||
1. Go to the `BotRequestHandler` class.
|
||||
1. Replace the content of the method `process_image` to extract and process an image from the incoming message:
|
||||
```python
|
||||
# Check if there is an image
|
||||
image_url = BotRequestHandler.get_image_url(activity.attachments)
|
||||
|
||||
if image_url:
|
||||
dominant_color = ComputerVisionApiService.analyze_image(image_url)
|
||||
response = await BotRequestHandler.create_reply_activity(activity, f'Do you need a {dominant_color} cube? Let me find one for you!')
|
||||
await context.send_activity(response)
|
||||
BotCommandHandler.move_cube(dominant_color)
|
||||
else:
|
||||
response = await BotRequestHandler.create_reply_activity(activity, 'Please provide a valid instruction or image.')
|
||||
await context.send_activity(response)
|
||||
```
|
||||
1. Go to the `ComputerVisionApiService` class.
|
||||
1. Modify the method `analyze_image`:
|
||||
* Add the following code after the line `Request headers and parameters`
|
||||
```python
|
||||
headers = {
|
||||
'Ocp-Apim-Subscription-Key': COMPUTER_VISION_SUBSCRIPTION_KEY,
|
||||
'Content-Type': 'application/octet-stream'
|
||||
}
|
||||
params = {'visualFeatures': 'Color'}
|
||||
```
|
||||
* Add the following code after the line `Get image bytes content` to get the image content as bytes:
|
||||
```python
|
||||
image_data = BytesIO(requests.get(image_url).content)
|
||||
```
|
||||
* Replace the line `return None` with the following code snippet
|
||||
```python
|
||||
print(f'Processing image: {image_url}')
|
||||
response = requests.post(COMPUTER_VISION_ANALYZE_URL, headers=headers, params=params, data=image_data)
|
||||
response.raise_for_status()
|
||||
analysis = response.json()
|
||||
dominant_color = analysis["color"]["dominantColors"][0]
|
||||
|
||||
return dominant_color
|
||||
```
|
||||
|
||||
### Add the robot code
|
||||
|
||||
1. Add the following code snippet to the `move_cube` method (**talk-to-my-robot.py** file):
|
||||
```python
|
||||
print(f'Moving {color} cube...')
|
||||
try:
|
||||
r = requests.get(f'{SIM_API_HOST}/put_block_into_tray/{color}/1')
|
||||
r.raise_for_status()
|
||||
print('done moving cube . . .')
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
```
|
||||
1. Save the **talk-to-my-robot.py** file.
|
||||
|
||||
### Test the move cube command
|
||||
|
||||
1. Select **Debug -> Start Without Debugging** then click **Python** to execute the bot script in **VSCode**.
|
||||
1. Go back to the **Bot Framework Emulator** app.
|
||||
1. Click **Start Over** to start a new conversation.
|
||||
1. Click the upload button from the left bottom corner to upload an image.
|
||||
1. Select the file `~/AI-Robot-Challenge-Lab/resources/Images/cube-red.png`.
|
||||
1. Return to **Gazebo** and wait for the simulator to move the requested cube.
|
||||
1. Go back to the **Bot Framework Emulator** app.
|
||||
1. Select another image of a different color and check the simulator to verify which cube it moved.
|
||||
|
||||
|
||||
# Contributing
|
||||
|
||||
|
|
@ -12,3 +439,5 @@ provided by the bot. You will only need to do this once across all repos using o
|
|||
This project has adopted the [Microsoft Open Source Code of Conduct](https://opensource.microsoft.com/codeofconduct/).
|
||||
For more information see the [Code of Conduct FAQ](https://opensource.microsoft.com/codeofconduct/faq/) or
|
||||
contact [opencode@microsoft.com](mailto:opencode@microsoft.com) with any additional questions or comments.
|
||||
|
||||
|
||||
|
|
|
|||
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 31 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 30 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 30 KiB |
|
|
@ -0,0 +1,97 @@
|
|||
{
|
||||
"luis_schema_version": "3.0.0",
|
||||
"versionId": "0.1",
|
||||
"name": "robotics-demo",
|
||||
"desc": "",
|
||||
"culture": "en-us",
|
||||
"intents": [
|
||||
{
|
||||
"name": "MoveArm"
|
||||
},
|
||||
{
|
||||
"name": "MoveGrippers"
|
||||
},
|
||||
{
|
||||
"name": "None"
|
||||
},
|
||||
{
|
||||
"name": "ShowStats"
|
||||
}
|
||||
],
|
||||
"entities": [],
|
||||
"composites": [],
|
||||
"closedLists": [
|
||||
{
|
||||
"name": "action",
|
||||
"subLists": [
|
||||
{
|
||||
"canonicalForm": "open",
|
||||
"list": []
|
||||
},
|
||||
{
|
||||
"canonicalForm": "close",
|
||||
"list": []
|
||||
}
|
||||
],
|
||||
"roles": []
|
||||
}
|
||||
],
|
||||
"patternAnyEntities": [],
|
||||
"regex_entities": [],
|
||||
"prebuiltEntities": [],
|
||||
"model_features": [],
|
||||
"regex_features": [],
|
||||
"patterns": [],
|
||||
"utterances": [
|
||||
{
|
||||
"text": "can you move your arm?",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "close the grippers",
|
||||
"intent": "MoveGrippers",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "move arm",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "move arm robot",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "move your arm in circles",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "open grippers",
|
||||
"intent": "MoveGrippers",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "please move your arm",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "show me your statistics",
|
||||
"intent": "ShowStats",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "show stats",
|
||||
"intent": "ShowStats",
|
||||
"entities": []
|
||||
},
|
||||
{
|
||||
"text": "wave arm",
|
||||
"intent": "MoveArm",
|
||||
"entities": []
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,448 @@
|
|||
#!/bin/bash
|
||||
|
||||
# Debug - Break on error
|
||||
set -e
|
||||
|
||||
# Hack: Clear any file locks
|
||||
sudo rm /var/lib/apt/lists/lock
|
||||
|
||||
# Color it
|
||||
NC='\033[0m' # No Color
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
BLUE='\033[0;34m'
|
||||
|
||||
# Time it
|
||||
scriptstart=$(date +%s)
|
||||
|
||||
#
|
||||
# ROS image for Sawyer development
|
||||
#
|
||||
|
||||
# author="Paul Stubbs (pstubbs@microsoft.com)"
|
||||
# description="Microsoft Robot Challenge 2018"
|
||||
# version="1.0"
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Bot Framework + Emulator
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall bot framework + Emulator\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
|
||||
sudo apt-get update
|
||||
# install unmet dependencies
|
||||
sudo apt-get -f install -y
|
||||
sudo apt-get -f install libindicator7 -y
|
||||
sudo apt-get -f install libappindicator1 -y
|
||||
# install any unmet dependencies
|
||||
sudo apt-get -f install -y
|
||||
|
||||
TEMP_DEB="$(mktemp)"
|
||||
wget -O "$TEMP_DEB" 'https://github.com/Microsoft/BotFramework-Emulator/releases/download/v4.0.15-alpha/botframework-emulator_4.0.15-alpha_amd64.deb'
|
||||
sudo dpkg -i "$TEMP_DEB"
|
||||
rm -f "$TEMP_DEB"
|
||||
# install any unmet dependencies
|
||||
sudo apt-get -f install -y
|
||||
sudo apt autoremove -y
|
||||
|
||||
# Install Python 3.6
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Python 3.6\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
sudo add-apt-repository -y ppa:deadsnakes/ppa
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y python3.6
|
||||
sudo apt-get install -y python3-pip
|
||||
# install any unmet dependencies
|
||||
sudo apt-get -f install -y
|
||||
python3.6 -m pip install --upgrade pip
|
||||
|
||||
# Install Bot Framework Deps
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall bot framework Dependencies\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
|
||||
python3.6 -m pip install --user aiohttp
|
||||
python3.6 -m pip install --user requests
|
||||
python3.6 -m pip install --user botbuilder.schema
|
||||
python3.6 -m pip install --user botbuilder.core
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install ROS
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall ROS\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
# http://sdk.rethinkrobotics.com/intera/Workstation_Setup
|
||||
|
||||
# Update to lateset software lists
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
|
||||
# Install some common CLI tools
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y wget software-properties-common
|
||||
|
||||
# Configure Ubuntu repositories. Setup sources.list
|
||||
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
|
||||
|
||||
# Setup keys
|
||||
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
|
||||
|
||||
# Install ROS Kinetic Desktop FUll
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\n Install ROS Kinetic Desktop FUll \n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y ros-kinetic-desktop-full
|
||||
|
||||
# Initialize rosdep
|
||||
sudo rosdep init || echo -e "${YELLOW}ROSDep Already Exists.${NC}"
|
||||
rosdep update
|
||||
|
||||
# Install rosinstall
|
||||
sudo apt-get install -y python-rosinstall -y
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Create Development Workspace
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nCreate Development Workspace\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
#Add the path to ROS
|
||||
echo "source ~/ros_ws/devel/setup.bash" >> ~/.bashrc
|
||||
|
||||
|
||||
# Create ROS Workspace
|
||||
mkdir -p ~/ros_ws/src
|
||||
cd ~/ros_ws
|
||||
source /opt/ros/kinetic/setup.bash
|
||||
catkin_make
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Intera SDK Dependencies
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Intera SDK Dependencies\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
# Install SDK Dependencies
|
||||
# Update to lateset software lists
|
||||
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y \
|
||||
git-core \
|
||||
python-argparse \
|
||||
python-wstool \
|
||||
python-vcstools \
|
||||
python-rosdep \
|
||||
ros-kinetic-control-msgs \
|
||||
ros-kinetic-joystick-drivers \
|
||||
ros-kinetic-xacro \
|
||||
ros-kinetic-tf2-ros \
|
||||
ros-kinetic-rviz \
|
||||
ros-kinetic-cv-bridge \
|
||||
ros-kinetic-actionlib \
|
||||
ros-kinetic-actionlib-msgs \
|
||||
ros-kinetic-dynamic-reconfigure \
|
||||
ros-kinetic-trajectory-msgs \
|
||||
ros-kinetic-rospy-message-converter
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Intera Robot SDK
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Intera Robot SDK\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
cd ~/ros_ws/src
|
||||
wstool init .
|
||||
git clone https://github.com/RethinkRobotics/sawyer_robot.git
|
||||
wstool merge sawyer_robot/sawyer_robot.rosinstall
|
||||
wstool update
|
||||
|
||||
# Source ROS Setup
|
||||
cd ~/ros_ws
|
||||
source /opt/ros/kinetic/setup.bash
|
||||
catkin_make
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Configure Robot Communication/ROS Workspace
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nConfigure Robot Communication/ROS Workspace\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
# Copy the intera.sh script
|
||||
# The intera.sh file already exists in intera_sdk repo,
|
||||
# copy the file into your ros workspace.
|
||||
cp ~/ros_ws/src/intera_sdk/intera.sh ~/ros_ws
|
||||
|
||||
# Update the copy of the intera.sh file
|
||||
# cd ~/ros_ws
|
||||
# Update ROS Distribution
|
||||
sed -i 's/ros_version="indigo"/ros_version="kinetic"/' ~/ros_ws/intera.sh
|
||||
# Update the ROBOTS hostname
|
||||
sed -i 's/robot_hostname="robot_hostname.local"/robot_hostname="paule.local"/' ~/ros_ws/intera.sh
|
||||
|
||||
# TODO:// Need to figure out the docker networking to resolve hostname
|
||||
# Update YOUR IP or Hostname. This must be resolvable to the Robot
|
||||
# Choose one. Be sure to add or remove the leading # from the right ones
|
||||
sed -i 's/your_ip="192.168.XXX.XXX"/your_ip="192.168.XXX.XXX"/' ~/ros_ws/intera.sh
|
||||
#sed -i 's/#your_hostname="my_computer.local"/your_hostname="my_computer.local"/' intera.sh
|
||||
|
||||
# Setup and configure RVIZ
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nSetup and configure RVIZ\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# TODO:// need to do this still
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
# http://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial
|
||||
#
|
||||
#
|
||||
#
|
||||
# Configure Sawyer with Gazebo
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
# Install Prerequisites
|
||||
# Update to lateset software lists
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y \
|
||||
gazebo7 \
|
||||
ros-kinetic-qt-build \
|
||||
ros-kinetic-gazebo-ros-control \
|
||||
ros-kinetic-gazebo-ros-pkgs \
|
||||
ros-kinetic-ros-control \
|
||||
ros-kinetic-control-toolbox \
|
||||
ros-kinetic-realtime-tools \
|
||||
ros-kinetic-ros-controllers \
|
||||
ros-kinetic-xacro \
|
||||
python-wstool \
|
||||
ros-kinetic-tf-conversions \
|
||||
ros-kinetic-kdl-parser \
|
||||
ros-kinetic-sns-ik-lib
|
||||
|
||||
# Install Sawyer Simulator files
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Sawyer Simulator files\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
|
||||
cd ~/ros_ws/src
|
||||
if [ ! -d src/sawyer_simulator ]
|
||||
then
|
||||
# folder does not exist so clone the repo
|
||||
echo -e ${YELLOW}
|
||||
echo "~/ros_ws/src/sawyer_simulator folder does not exist, cloning now"
|
||||
echo -e ${NC}
|
||||
git clone https://github.com/RethinkRobotics/sawyer_simulator.git
|
||||
else
|
||||
# folder does exist so pull to update
|
||||
echo -e ${YELLOW}
|
||||
echo "~/ros_ws/src/sawyer_simulator folder already exists, updating now"
|
||||
echo -e ${NC}
|
||||
cd ~/ros_ws/src/sawyer_simulator
|
||||
git pull
|
||||
cd ~/ros_ws/src
|
||||
fi
|
||||
|
||||
source /opt/ros/kinetic/setup.bash
|
||||
wstool merge sawyer_simulator/sawyer_simulator.rosinstall
|
||||
wstool update
|
||||
|
||||
# Build the Sources
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nSDK Build the Sources\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
source /opt/ros/kinetic/setup.bash
|
||||
cd ~/ros_ws
|
||||
catkin_make
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install other tools
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Chromium Browser
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Chromium Browser\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
#https://www.linuxbabe.com/ubuntu/install-google-chrome-ubuntu-16-04-lts
|
||||
sudo su -c "echo 'deb [arch=amd64] http://dl.google.com/linux/chrome/deb/ stable main' >> /etc/apt/sources.list"
|
||||
TEMP_DEB="$(mktemp)"
|
||||
wget -O "$TEMP_DEB" 'https://dl.google.com/linux/linux_signing_key.pub'
|
||||
sudo apt-key add "$TEMP_DEB"
|
||||
rm -f "$TEMP_DEB"
|
||||
sudo apt-get update
|
||||
sudo apt-get install google-chrome-stable
|
||||
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Visual Studio Code
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nInstall Visual Studio Code\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
# Time it
|
||||
start=$(date +%s)
|
||||
#
|
||||
#
|
||||
#
|
||||
#https://tecadmin.net/install-visual-studio-code-editor-ubuntu/
|
||||
sudo su -c "echo 'deb [arch=amd64] http://packages.microsoft.com/repos/vscode stable main' >> /etc/apt/sources.list.d/vscode.list"
|
||||
curl https://packages.microsoft.com/keys/microsoft.asc | gpg --dearmor > microsoft.gpg
|
||||
sudo mv microsoft.gpg /etc/apt/trusted.gpg.d/microsoft.gpg
|
||||
sudo apt-get update
|
||||
sudo apt-get install code
|
||||
|
||||
# Time it
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
# Update the machine
|
||||
echo -e ${GREEN}
|
||||
echo -e "***\n***\n***\n***\nUpdate the Machine\n***\n***\n***\n***"
|
||||
echo -e ${NC}
|
||||
#
|
||||
#
|
||||
#
|
||||
sudo apt-get update
|
||||
sudo apt-get upgrade -y
|
||||
sudo apt-get dist-upgrade -y
|
||||
sudo apt-get autoremove -y
|
||||
sudo apt-get autoclean -y
|
||||
|
||||
# Time it
|
||||
start=${scriptstart}
|
||||
end=$(date +%s)
|
||||
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
|
||||
echo -e ${BLUE}
|
||||
echo -e "Total Elapsed Time: ${runtime}"
|
||||
echo -e ${NC}
|
||||
|
|
@ -0,0 +1,595 @@
|
|||
<?xml version="1.0" ?><!-- =================================================================================== --><!-- | This document was autogenerated by xacro from ./urdf.xacro | --><!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --><!-- =================================================================================== --><robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="darkgray">
|
||||
<color rgba=".2 .2 .2 1"/>
|
||||
</material>
|
||||
<material name="darkred">
|
||||
<color rgba=".5 .1 .1 1"/>
|
||||
</material>
|
||||
<material name="sawyer_red">
|
||||
<color rgba=".5 .1 .1 1"/>
|
||||
</material>
|
||||
<material name="sawyer_gray">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
<link name="base"/>
|
||||
<link name="torso">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0 0 0"/>
|
||||
</geometry>
|
||||
<material name="darkgray">
|
||||
<color rgba=".2 .2 .2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="pedestal">
|
||||
<visual>
|
||||
<origin rpy="1.57079632679 0 -1.57079632679" xyz="0.26 0.345 -0.91488"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/pedestal.DAE"/>
|
||||
</geometry>
|
||||
<material name="darkgray"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.02 0 -0.29"/>
|
||||
<geometry>
|
||||
<cylinder length="0.62" radius="0.18"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="60.86397744"/>
|
||||
<inertia ixx="5.0635929" ixy="0.00103417" ixz="0.80199628" iyy="6.08689388" iyz="0.00105311" izz="4.96191932"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="torso_t0" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base"/>
|
||||
<child link="torso"/>
|
||||
</joint>
|
||||
<joint name="pedestal_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="base"/>
|
||||
<child link="pedestal"/>
|
||||
</joint>
|
||||
<joint name="right_arm_mount" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="base"/>
|
||||
<child link="right_arm_base_link"/>
|
||||
</joint>
|
||||
<link name="right_arm_base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0006241 -2.8025E-05 0.065404"/>
|
||||
<mass value="2.0687"/>
|
||||
<inertia ixx="0.0067599" ixy="-4.2024E-05" ixz="-6.1904E-07" iyy="0.0067877" iyz="1.5888E-05" izz="0.0074031"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/base.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.12"/>
|
||||
<geometry>
|
||||
<cylinder length="0.24" radius="0.08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_l0">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.024366 0.010969 0.14363"/>
|
||||
<mass value="5.3213"/>
|
||||
<inertia ixx="0.053314" ixy="0.0047093" ixz="0.011734" iyy="0.057902" iyz="0.0080179" izz="0.023659"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_mp3/l0.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.08 0 0.23"/>
|
||||
<geometry>
|
||||
<sphere radius="0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j0" type="revolute">
|
||||
<origin rpy="0.0002145211 0.0019869535 -4.784e-06" xyz="0.0005951423 7.50436e-05 0.0797496365"/>
|
||||
<parent link="right_arm_base_link"/>
|
||||
<child link="right_l0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="80.0" lower="-3.0503" upper="3.0503" velocity="1.74"/>
|
||||
</joint>
|
||||
<link name="head">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0053207 -2.6549E-05 0.1021"/>
|
||||
<mass value="1.5795"/>
|
||||
<inertia ixx="0.011833" ixy="-4.4669E-06" ixz="4.9425E-05" iyy="0.0082709" iyz="4.2124E-07" izz="0.0049661"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/head.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.08"/>
|
||||
<geometry>
|
||||
<sphere radius="0.18"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="head_pan" type="revolute">
|
||||
<origin rpy="-0.000671675137446 -0.000304036251317 0.0961528036545" xyz="2.25041348411e-06 0.000381519444166 0.2965"/>
|
||||
<parent link="right_l0"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="8" lower="-5.0952" upper="0.9064" velocity="1.8"/>
|
||||
</joint>
|
||||
<link name="right_torso_itb">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_torso_itb" type="fixed">
|
||||
<origin rpy="0 -1.57079632679 0" xyz="-.055 0 .22"/>
|
||||
<parent link="right_l0"/>
|
||||
<child link="right_torso_itb"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="right_l1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0030849 -0.026811 0.092521"/>
|
||||
<mass value="4.505"/>
|
||||
<inertia ixx="0.022398" ixy="-0.00023986" ixz="-0.00029362" iyy="0.014613" iyz="-0.0060875" izz="0.017295"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_mp3/l1.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1225"/>
|
||||
<geometry>
|
||||
<sphere radius="0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j1" type="revolute">
|
||||
<origin rpy="-1.56975753069 1.57057976739 -0.0010821609" xyz="0.0809015792 0.0498854227 0.237"/>
|
||||
<parent link="right_l0"/>
|
||||
<child link="right_l1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="80.0" lower="-3.8095" upper="2.2736" velocity="1.328"/>
|
||||
</joint>
|
||||
<link name="right_l2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00016044 -0.014967 0.13582"/>
|
||||
<mass value="1.745"/>
|
||||
<inertia ixx="0.025506" ixy="4.4101E-06" ixz="1.4955E-05" iyy="0.0253" iyz="-0.0033204" izz="0.0034179"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l2.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.08"/>
|
||||
<geometry>
|
||||
<cylinder length="0.34" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j2" type="revolute">
|
||||
<origin rpy="1.57061011019 -0.000961752 0.0012245467" xyz="-0.0005275357 -0.1400963995 0.1423808458"/>
|
||||
<parent link="right_l1"/>
|
||||
<child link="right_l2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40.0" lower="-3.0426" upper="3.0426" velocity="1.957"/>
|
||||
</joint>
|
||||
<link name="right_l3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0048135 -0.0281 -0.084154"/>
|
||||
<mass value="2.5097"/>
|
||||
<inertia ixx="0.01016" ixy="-9.7452E-06" ixz="0.00026624" iyy="0.0065685" iyz="0.0030316" izz="0.0069078"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l3.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 -0.01 -0.12"/>
|
||||
<geometry>
|
||||
<sphere radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j3" type="revolute">
|
||||
<origin rpy="-1.57210568599 0.0005985171 0.0005895877" xyz="-0.0002017802 -0.0423919721 0.2599891702"/>
|
||||
<parent link="right_l2"/>
|
||||
<child link="right_l3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40.0" lower="-3.0439" upper="3.0439" velocity="1.957"/>
|
||||
</joint>
|
||||
<link name="right_l4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0018844 0.0069001 0.1341"/>
|
||||
<mass value="1.1136"/>
|
||||
<inertia ixx="0.013557" ixy="1.8109E-05" ixz="0.00013523" iyy="0.013555" iyz="0.0010561" izz="0.0013658"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l4.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.11"/>
|
||||
<geometry>
|
||||
<cylinder length="0.30" radius="0.045"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j4" type="revolute">
|
||||
<origin rpy="1.57046530879 0.0010608855 0.0002608344" xyz="0.0004098916 -0.1249007494 -0.1269097331"/>
|
||||
<parent link="right_l3"/>
|
||||
<child link="right_l4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
|
||||
</joint>
|
||||
<link name="right_arm_itb">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_arm_itb" type="fixed">
|
||||
<origin rpy="0 -1.57079632679 0" xyz="-.055 0 .075"/>
|
||||
<parent link="right_l4"/>
|
||||
<child link="right_arm_itb"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="right_l5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0061133 -0.023697 0.076416"/>
|
||||
<mass value="1.5625"/>
|
||||
<inertia ixx="0.0047328" ixy="0.00011526" ixz="4.6269E-05" iyy="0.0029676" iyz="-0.0011557" izz="0.0031762"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l5.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1"/>
|
||||
<geometry>
|
||||
<sphere radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j5" type="revolute">
|
||||
<origin rpy="-1.56764146979 0.0008153652 -0.0005184882" xyz="-0.0001775417 0.0308361277 0.2748611106"/>
|
||||
<parent link="right_l4"/>
|
||||
<child link="right_l5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
|
||||
</joint>
|
||||
<link name="right_hand_camera"/>
|
||||
<joint name="right_hand_camera" type="fixed">
|
||||
<origin rpy="0.769478294976 1.55880975915 0.764106410692" xyz="0.0379579844756 -0.0325006158013 0.0686967641527"/>
|
||||
<parent link="right_l5"/>
|
||||
<child link="right_hand_camera"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="right_wrist"/>
|
||||
<joint name="right_wrist" type="fixed">
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0.10541"/>
|
||||
<parent link="right_l5"/>
|
||||
<child link="right_wrist"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="right_l6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-8.0726E-06 0.0085838 -0.0049566"/>
|
||||
<mass value="0.3292"/>
|
||||
<inertia ixx="0.00031105" ixy="1.4771E-06" ixz="-3.7074E-07" iyy="0.00021549" iyz="-8.4533E-06" izz="0.00035976"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://sawyer_description/meshes/sawyer_mp1/l6.DAE"/>
|
||||
</geometry>
|
||||
<material name="sawyer_red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.015 -0.01"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.055"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j6" type="revolute">
|
||||
<origin rpy="-1.57238452459 -0.1781653283 3.1424031454" xyz="-2.46636e-05 -0.1098548426 0.1050918016"/>
|
||||
<parent link="right_l5"/>
|
||||
<child link="right_l6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9.0" lower="-4.7124" upper="4.7124" velocity="4.545"/>
|
||||
</joint>
|
||||
<link name="right_hand">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
|
||||
<mass value="1.0E-08"/>
|
||||
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_hand" type="fixed">
|
||||
<origin rpy="0 0 1.570796" xyz="0 0 0.0245"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_l6"/>
|
||||
<child link="right_hand"/>
|
||||
</joint>
|
||||
<link name="right_l1_2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
|
||||
<mass value="1.0E-08"/>
|
||||
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.035"/>
|
||||
<geometry>
|
||||
<cylinder length="0.14" radius="0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j1_2" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="right_l1"/>
|
||||
<child link="right_l1_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="right_l2_2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
|
||||
<mass value="1.0E-08"/>
|
||||
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.26"/>
|
||||
<geometry>
|
||||
<sphere radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j2_2" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="right_l2"/>
|
||||
<child link="right_l2_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="right_l4_2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
|
||||
<mass value="1.0E-08"/>
|
||||
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.01 0.26"/>
|
||||
<geometry>
|
||||
<sphere radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_j4_2" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="right_l4"/>
|
||||
<child link="right_l4_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="screen">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.24 0.14 0.002"/>
|
||||
</geometry>
|
||||
<material name="darkgray"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="display_joint" type="fixed">
|
||||
<origin rpy="1.57079632679 0 1.57079632679" xyz="0.03 0.0 0.105"/>
|
||||
<parent link="head"/>
|
||||
<child link="screen"/>
|
||||
</joint>
|
||||
<link name="head_camera"/>
|
||||
<joint name="head_camera" type="fixed">
|
||||
<origin rpy="-2.12510633299 -0.019081815459 -1.57079632679" xyz="0.0234787515859 0.00134193139039 0.215168253323"/>
|
||||
<parent link="head"/>
|
||||
<child link="head_camera"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="controller_box">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.325 0 -0.38"/>
|
||||
<geometry>
|
||||
<box size="0.22 0.4 0.53"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="controller_box_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="base"/>
|
||||
<child link="controller_box"/>
|
||||
</joint>
|
||||
<link name="pedestal_feet">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/>
|
||||
<geometry>
|
||||
<box size="0.77 0.7 0.31"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="pedestal_feet_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="base"/>
|
||||
<child link="pedestal_feet"/>
|
||||
</joint>
|
||||
|
||||
<joint name="stp_021804TP00031_base_joint" type="fixed">
|
||||
<child link="stp_021804TP00031_base">
|
||||
</child>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.005">
|
||||
</origin>
|
||||
<parent link="right_hand">
|
||||
</parent>
|
||||
</joint>
|
||||
<joint name="stp_021804TP00031_tip_1_joint" type="fixed">
|
||||
<child link="stp_021804TP00031_tip_1">
|
||||
</child>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
<parent link="stp_021804TP00031_base">
|
||||
</parent>
|
||||
</joint>
|
||||
<joint name="stp_021804TP00031_tip_2_joint" type="fixed">
|
||||
<child link="stp_021804TP00031_tip_2">
|
||||
</child>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
<parent link="stp_021804TP00031_base">
|
||||
</parent>
|
||||
</joint>
|
||||
<link name="stp_021804TP00031_base">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.0269" radius="0.070">
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.01845">
|
||||
</origin>
|
||||
</collision>
|
||||
<inertial>
|
||||
<inertia ixx="0.00016416" ixy="0" ixz="0" iyy="0.00025526" iyz="0" izz="0.00035964">
|
||||
</inertia>
|
||||
<mass value="0.31382">
|
||||
</mass>
|
||||
<origin rpy="0 0 0" xyz="0.0085294 0 0.015587">
|
||||
</origin>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://intera_tools_description/meshes/smart_tool_plate/smart_tool_plate.DAE">
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0 0 0 1">
|
||||
</color>
|
||||
</material>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="stp_021804TP00031_tip_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01">
|
||||
</box>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</collision>
|
||||
<inertial>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0">
|
||||
</inertia>
|
||||
<mass value="0">
|
||||
</mass>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.015 0.015 0.015">
|
||||
</box>
|
||||
</geometry>
|
||||
<material name="darkgrey">
|
||||
<color rgba="0.2 0.3 0.3 1">
|
||||
</color>
|
||||
</material>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="stp_021804TP00031_tip_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01">
|
||||
</box>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</collision>
|
||||
<inertial>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0">
|
||||
</inertia>
|
||||
<mass value="0">
|
||||
</mass>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.015 0.015 0.015">
|
||||
</box>
|
||||
</geometry>
|
||||
<material name="darkgrey">
|
||||
<color rgba="0.2 0.3 0.3 1">
|
||||
</color>
|
||||
</material>
|
||||
<origin rpy="0 0 0" xyz="0 0 0">
|
||||
</origin>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright (c) 2013-2018, Rethink Robotics Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
"""
|
||||
SDK Gripper Example: keyboard
|
||||
"""
|
||||
import argparse
|
||||
|
||||
import rospy
|
||||
import time
|
||||
|
||||
import intera_interface
|
||||
import intera_external_devices
|
||||
from intera_interface import CHECK_VERSION
|
||||
|
||||
def move_gripper(limb, action):
|
||||
# initialize interfaces
|
||||
print("Getting robot state...")
|
||||
rs = intera_interface.RobotEnable(CHECK_VERSION)
|
||||
init_state = rs.state()
|
||||
gripper = None
|
||||
original_deadzone = None
|
||||
|
||||
def clean_shutdown():
|
||||
if gripper and original_deadzone:
|
||||
gripper.set_dead_zone(original_deadzone)
|
||||
print("Exiting example.")
|
||||
|
||||
try:
|
||||
gripper = intera_interface.Gripper(limb + '_gripper')
|
||||
except (ValueError, OSError) as e:
|
||||
rospy.logerr("Could not detect an electric gripper attached to the robot.")
|
||||
clean_shutdown()
|
||||
return
|
||||
rospy.on_shutdown(clean_shutdown)
|
||||
|
||||
original_deadzone = gripper.get_dead_zone()
|
||||
# WARNING: setting the deadzone below this can cause oscillations in
|
||||
# the gripper position. However, setting the deadzone to this
|
||||
# value is required to achieve the incremental commands in this example
|
||||
gripper.set_dead_zone(0.001)
|
||||
rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
|
||||
rospy.loginfo("Enabling robot...")
|
||||
rs.enable()
|
||||
print("Controlling grippers.")
|
||||
if (action == "open"):
|
||||
gripper.open()
|
||||
rospy.sleep(1.0)
|
||||
print("Opened grippers")
|
||||
elif (action == "close"):
|
||||
gripper.close()
|
||||
rospy.sleep(1.0)
|
||||
print("Closed grippers")
|
||||
|
||||
# force shutdown call if caught by key handler
|
||||
rospy.signal_shutdown("Example finished.")
|
||||
|
||||
|
||||
def main():
|
||||
"""RSDK Gripper Example: send a command to control the grippers.
|
||||
|
||||
Run this example to command various gripper movements while
|
||||
adjusting gripper parameters, including calibration, and velocity:
|
||||
Uses the intera_interface.Gripper class and the
|
||||
helper function, intera_external_devices.getch.
|
||||
"""
|
||||
epilog = """
|
||||
See help inside the example with the '?' key for key bindings.
|
||||
"""
|
||||
rp = intera_interface.RobotParams()
|
||||
valid_limbs = rp.get_limb_names()
|
||||
if not valid_limbs:
|
||||
rp.log_message(("Cannot detect any limb parameters on this robot. "
|
||||
"Exiting."), "ERROR")
|
||||
return
|
||||
limb = valid_limbs[0]
|
||||
print("Using limb: {}.".format(limb))
|
||||
arg_fmt = argparse.RawDescriptionHelpFormatter
|
||||
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
|
||||
description=main.__doc__,
|
||||
epilog=epilog)
|
||||
parser.add_argument(
|
||||
"-a", "--action", dest="action", default="open",
|
||||
choices=["open", "close"],
|
||||
help="Action to perform with the gripper. Options: close or open"
|
||||
)
|
||||
args = parser.parse_args(rospy.myargv()[1:])
|
||||
|
||||
print("Initializing node... ")
|
||||
rospy.init_node("sdk_gripper_keyboard")
|
||||
|
||||
move_gripper(limb, args.action)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import sys
|
||||
import rospy
|
||||
import intera_interface
|
||||
|
||||
from std_msgs.msg import String
|
||||
from intera_interface import CHECK_VERSION
|
||||
|
||||
|
||||
def main():
|
||||
rospy.init_node('Demo_Stats')
|
||||
rs = intera_interface.RobotEnable(CHECK_VERSION)
|
||||
state = rs.state()
|
||||
print state
|
||||
rospy.signal_shutdown("Demo_Stats finished.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import json
|
||||
|
||||
# rospy - ROS Python API
|
||||
import rospy
|
||||
|
||||
# intera_interface - Sawyer Python API
|
||||
import intera_interface
|
||||
|
||||
# initialize our ROS node, registering it with the Master
|
||||
rospy.init_node('Hello_Sawyer')
|
||||
|
||||
# create an instance of intera_interface's Limb class
|
||||
limb = intera_interface.Limb('right')
|
||||
|
||||
# get the right limb's current joint angles
|
||||
angles = limb.joint_angles()
|
||||
|
||||
# print the current joint angles as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
# move to neutral pose
|
||||
limb.move_to_neutral()
|
||||
|
||||
# get the right limb's current joint angles now that it is in neutral
|
||||
angles = limb.joint_angles()
|
||||
|
||||
# print the current joint angles again as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
|
||||
# reassign new joint angles (all zeros) which we will later command to the limb
|
||||
angles['right_j0']=0.0
|
||||
angles['right_j1']=0.0
|
||||
angles['right_j2']=0.0
|
||||
angles['right_j3']=0.0
|
||||
angles['right_j4']=0.0
|
||||
angles['right_j5']=0.0
|
||||
angles['right_j6']=0.0
|
||||
|
||||
# print the joint angle command as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
# move the right arm to those joint angles
|
||||
limb.move_to_joint_positions(angles)
|
||||
|
||||
# Sawyer wants to say hello, let's wave the arm
|
||||
|
||||
# store the first wave position
|
||||
wave_1 = {'right_j6': -1.5126, 'right_j5': -0.3438, 'right_j4': 1.5126, 'right_j3': -1.3833, 'right_j2': 0.03726, 'right_j1': 0.3526, 'right_j0': -0.4259}
|
||||
|
||||
# store the second wave position
|
||||
wave_2 = {'right_j6': -1.5101, 'right_j5': -0.3806, 'right_j4': 1.5103, 'right_j3': -1.4038, 'right_j2': -0.2609, 'right_j1': 0.3940, 'right_j0': -0.4281}
|
||||
|
||||
|
||||
# wave three times
|
||||
for _move in range(3):
|
||||
limb.move_to_joint_positions(wave_1)
|
||||
rospy.sleep(0.5)
|
||||
limb.move_to_joint_positions(wave_2)
|
||||
rospy.sleep(0.5)
|
||||
|
|
@ -0,0 +1,226 @@
|
|||
import json
|
||||
import requests
|
||||
import re
|
||||
from io import BytesIO
|
||||
|
||||
# Used to call Python 2.7 from 3.6
|
||||
import subprocess
|
||||
|
||||
from aiohttp import web
|
||||
from botbuilder.core import (BotFrameworkAdapter, BotFrameworkAdapterSettings, TurnContext)
|
||||
from botbuilder.schema import (Activity, ActivityTypes)
|
||||
|
||||
|
||||
# Settings
|
||||
BOT_APP_ID = ''
|
||||
BOT_APP_PASSWORD = ''
|
||||
SETTINGS = BotFrameworkAdapterSettings(BOT_APP_ID, BOT_APP_PASSWORD)
|
||||
ADAPTER = BotFrameworkAdapter(SETTINGS)
|
||||
|
||||
LUIS_APP_ID = '357b6036-c961-4bb0-9aa9-bbbcdf1e32f0'
|
||||
LUIS_SUBSCRIPTION_KEY = 'b7f33ef452a544079fb86356479f9300'
|
||||
|
||||
COMPUTER_VISION_ANALYZE_URL = "https://westus.api.cognitive.microsoft.com/vision/v2.0/analyze"
|
||||
COMPUTER_VISION_SUBSCRIPTION_KEY = "f35e56ef483640fc9153d69c5c123266"
|
||||
|
||||
SIM_API_HOST = 'http://localhost:5000'
|
||||
|
||||
class ComputerVisionApiService:
|
||||
@staticmethod
|
||||
def analyze_image(image_url):
|
||||
# Request headers and parameters
|
||||
headers = {
|
||||
'Ocp-Apim-Subscription-Key': COMPUTER_VISION_SUBSCRIPTION_KEY,
|
||||
'Content-Type': 'application/octet-stream'
|
||||
}
|
||||
params = {'visualFeatures': 'Color'}
|
||||
|
||||
# Get image bytes content
|
||||
image_data = BytesIO(requests.get(image_url).content)
|
||||
|
||||
try:
|
||||
print('Processing image: {}'.format(image_url))
|
||||
response = requests.post(COMPUTER_VISION_ANALYZE_URL, headers=headers, params=params, data=image_data)
|
||||
response.raise_for_status()
|
||||
analysis = response.json()
|
||||
dominant_color = analysis["color"]["dominantColors"][0]
|
||||
|
||||
return dominant_color
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
|
||||
class LuisResponse():
|
||||
def __init__(self, intent, entity_value = None, entity_type = None):
|
||||
self.intent = intent
|
||||
self.entity_value = entity_value
|
||||
self.entity_type = entity_type
|
||||
|
||||
class LuisApiService:
|
||||
@staticmethod
|
||||
def post_utterance(message):
|
||||
# Request headers and parameters
|
||||
headers = {'Ocp-Apim-Subscription-Key': LUIS_SUBSCRIPTION_KEY}
|
||||
params = {
|
||||
# Query parameter
|
||||
'q': message,
|
||||
# Optional request parameters, set to default values
|
||||
'timezoneOffset': '0',
|
||||
'verbose': 'false',
|
||||
'spellCheck': 'false',
|
||||
'staging': 'false',
|
||||
}
|
||||
|
||||
try:
|
||||
r = requests.get('https://westus.api.cognitive.microsoft.com/luis/v2.0/apps/%s' % LUIS_APP_ID, headers=headers, params=params)
|
||||
topScoreIntent = r.json()['topScoringIntent']
|
||||
entities = r.json()['entities']
|
||||
intent = topScoreIntent['intent'] if topScoreIntent['score'] > 0.5 else 'None'
|
||||
entity = entities[0] if len(entities) > 0 else None
|
||||
|
||||
return LuisResponse(intent, entity['entity'], entity['type']) if entity else LuisResponse(intent)
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
|
||||
|
||||
class BotRequestHandler:
|
||||
async def create_reply_activity(request_activity, text) -> Activity:
|
||||
return Activity(
|
||||
type=ActivityTypes.message,
|
||||
channel_id=request_activity.channel_id,
|
||||
conversation=request_activity.conversation,
|
||||
recipient=request_activity.from_property,
|
||||
from_property=request_activity.recipient,
|
||||
text=text,
|
||||
service_url=request_activity.service_url)
|
||||
|
||||
async def handle_message(context: TurnContext) -> web.Response:
|
||||
activity = context.activity
|
||||
if activity.text:
|
||||
luis_result = LuisApiService.post_utterance(activity.text)
|
||||
|
||||
response = await BotRequestHandler.create_reply_activity(activity, f'Top Intent: {luis_result.intent}.')
|
||||
await context.send_activity(response)
|
||||
|
||||
if luis_result.intent == 'MoveArm':
|
||||
BotCommandHandler.move_arm()
|
||||
elif luis_result.intent == 'ShowStats':
|
||||
stats = BotCommandHandler.show_stats()
|
||||
response = await BotRequestHandler.create_reply_activity(activity, stats)
|
||||
await context.send_activity(response)
|
||||
elif luis_result.intent == 'MoveGrippers':
|
||||
BotCommandHandler.move_grippers(luis_result.entity_value)
|
||||
else:
|
||||
response = await BotRequestHandler.create_reply_activity(activity, 'Please provide a valid instruction')
|
||||
await context.send_activity(response)
|
||||
else:
|
||||
await BotRequestHandler.process_image(activity, context)
|
||||
|
||||
return web.Response(status=202)
|
||||
|
||||
async def handle_conversation_update(context: TurnContext) -> web.Response:
|
||||
if context.activity.members_added[0].id != context.activity.recipient.id:
|
||||
response = await BotRequestHandler.create_reply_activity(context.activity, 'Welcome to Sawyer Robot!')
|
||||
await context.send_activity(response)
|
||||
return web.Response(status=200)
|
||||
|
||||
async def unhandled_activity() -> web.Response:
|
||||
return web.Response(status=404)
|
||||
|
||||
async def process_image(activity: Activity, context: TurnContext):
|
||||
# Check if there is an image
|
||||
image_url = BotRequestHandler.get_image_url(activity.attachments)
|
||||
|
||||
if image_url:
|
||||
dominant_color = ComputerVisionApiService.analyze_image(image_url)
|
||||
response = await BotRequestHandler.create_reply_activity(activity, f'Do you need a {dominant_color} cube? Let me find one for you!')
|
||||
await context.send_activity(response)
|
||||
BotCommandHandler.move_cube(dominant_color)
|
||||
else:
|
||||
response = await BotRequestHandler.create_reply_activity(activity, 'Please provide a valid instruction or image.')
|
||||
await context.send_activity(response)
|
||||
|
||||
def get_image_url(attachments):
|
||||
p = re.compile('^image/(jpg|jpeg|png|gif)$')
|
||||
for attachment in attachments:
|
||||
rs = p.match(attachment.content_type)
|
||||
if rs:
|
||||
return attachment.content_url
|
||||
return None
|
||||
|
||||
|
||||
@staticmethod
|
||||
async def request_handler(context: TurnContext) -> web.Response:
|
||||
if context.activity.type == 'message':
|
||||
return await BotRequestHandler.handle_message(context)
|
||||
elif context.activity.type == 'conversationUpdate':
|
||||
return await BotRequestHandler.handle_conversation_update(context)
|
||||
else:
|
||||
return await BotRequestHandler.unhandled_activity()
|
||||
|
||||
async def messages(req: web.web_request) -> web.Response:
|
||||
body = await req.json()
|
||||
activity = Activity().deserialize(body)
|
||||
auth_header = req.headers['Authorization'] if 'Authorization' in req.headers else ''
|
||||
try:
|
||||
return await ADAPTER.process_activity(activity, auth_header, BotRequestHandler.request_handler)
|
||||
except Exception as e:
|
||||
raise e
|
||||
|
||||
class BotCommandHandler:
|
||||
def move_arm():
|
||||
print('Moving arm... do something cool')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-wave-arm-node.py"
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
|
||||
print('done moving arm . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + output.decode("utf-8"))
|
||||
|
||||
def show_stats():
|
||||
print('Showing stats... do something')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-stats-node.py"
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
result = output.decode("utf-8")
|
||||
|
||||
print('done getting state . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + result + '\n')
|
||||
return result
|
||||
|
||||
def move_cube(color):
|
||||
print(f'Moving {color} cube...')
|
||||
try:
|
||||
r = requests.get(f'{SIM_API_HOST}/put_block_into_tray/{color}/1')
|
||||
r.raise_for_status()
|
||||
print('done moving cube . . .')
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
|
||||
def move_grippers(action):
|
||||
print(f'{action} grippers... wait a few seconds')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-move-grippers.py -a {}".format(action)
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
|
||||
print('done moving grippers . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + output.decode("utf-8"))
|
||||
|
||||
|
||||
|
||||
app = web.Application()
|
||||
app.router.add_post('/api/messages', BotRequestHandler.messages)
|
||||
|
||||
try:
|
||||
web.run_app(app, host='localhost', port=9000)
|
||||
print('Started http server on localhost:9000')
|
||||
except Exception as e:
|
||||
raise e
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
{
|
||||
"name": "RoboticsBot",
|
||||
"description": "",
|
||||
"secretKey": "",
|
||||
"services": [
|
||||
{
|
||||
"appId": "",
|
||||
"id": "http://localhost:9000/api/messages",
|
||||
"type": "endpoint",
|
||||
"appPassword": "",
|
||||
"endpoint": "http://localhost:9000/api/messages",
|
||||
"name": "RoboticsBot"
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright (c) 2013-2018, Rethink Robotics Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
"""
|
||||
SDK Gripper Example: keyboard
|
||||
"""
|
||||
import argparse
|
||||
|
||||
import rospy
|
||||
import time
|
||||
|
||||
import intera_interface
|
||||
import intera_external_devices
|
||||
from intera_interface import CHECK_VERSION
|
||||
|
||||
def move_gripper(limb, action):
|
||||
# initialize interfaces
|
||||
print("Getting robot state...")
|
||||
rs = intera_interface.RobotEnable(CHECK_VERSION)
|
||||
init_state = rs.state()
|
||||
gripper = None
|
||||
original_deadzone = None
|
||||
|
||||
def clean_shutdown():
|
||||
if gripper and original_deadzone:
|
||||
gripper.set_dead_zone(original_deadzone)
|
||||
print("Exiting example.")
|
||||
|
||||
try:
|
||||
gripper = intera_interface.Gripper(limb + '_gripper')
|
||||
except (ValueError, OSError) as e:
|
||||
rospy.logerr("Could not detect an electric gripper attached to the robot.")
|
||||
clean_shutdown()
|
||||
return
|
||||
rospy.on_shutdown(clean_shutdown)
|
||||
|
||||
original_deadzone = gripper.get_dead_zone()
|
||||
# WARNING: setting the deadzone below this can cause oscillations in
|
||||
# the gripper position. However, setting the deadzone to this
|
||||
# value is required to achieve the incremental commands in this example
|
||||
gripper.set_dead_zone(0.001)
|
||||
rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
|
||||
rospy.loginfo("Enabling robot...")
|
||||
rs.enable()
|
||||
print("Controlling grippers.")
|
||||
if (action == "open"):
|
||||
gripper.open()
|
||||
rospy.sleep(1.0)
|
||||
print("Opened grippers")
|
||||
elif (action == "close"):
|
||||
gripper.close()
|
||||
rospy.sleep(1.0)
|
||||
print("Closed grippers")
|
||||
|
||||
# force shutdown call if caught by key handler
|
||||
rospy.signal_shutdown("Example finished.")
|
||||
|
||||
|
||||
def main():
|
||||
"""RSDK Gripper Example: send a command to control the grippers.
|
||||
|
||||
Run this example to command various gripper movements while
|
||||
adjusting gripper parameters, including calibration, and velocity:
|
||||
Uses the intera_interface.Gripper class and the
|
||||
helper function, intera_external_devices.getch.
|
||||
"""
|
||||
epilog = """
|
||||
See help inside the example with the '?' key for key bindings.
|
||||
"""
|
||||
rp = intera_interface.RobotParams()
|
||||
valid_limbs = rp.get_limb_names()
|
||||
if not valid_limbs:
|
||||
rp.log_message(("Cannot detect any limb parameters on this robot. "
|
||||
"Exiting."), "ERROR")
|
||||
return
|
||||
limb = valid_limbs[0]
|
||||
print("Using limb: {}.".format(limb))
|
||||
arg_fmt = argparse.RawDescriptionHelpFormatter
|
||||
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
|
||||
description=main.__doc__,
|
||||
epilog=epilog)
|
||||
parser.add_argument(
|
||||
"-a", "--action", dest="action", default="open",
|
||||
choices=["open", "close"],
|
||||
help="Action to perform with the gripper. Options: close or open"
|
||||
)
|
||||
args = parser.parse_args(rospy.myargv()[1:])
|
||||
|
||||
print("Initializing node... ")
|
||||
rospy.init_node("sdk_gripper_keyboard")
|
||||
|
||||
move_gripper(limb, args.action)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import sys
|
||||
import rospy
|
||||
import intera_interface
|
||||
|
||||
from std_msgs.msg import String
|
||||
from intera_interface import CHECK_VERSION
|
||||
|
||||
|
||||
def main():
|
||||
rospy.init_node('Demo_Stats')
|
||||
rs = intera_interface.RobotEnable(CHECK_VERSION)
|
||||
state = rs.state()
|
||||
print state
|
||||
rospy.signal_shutdown("Demo_Stats finished.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import json
|
||||
|
||||
# rospy - ROS Python API
|
||||
import rospy
|
||||
|
||||
# intera_interface - Sawyer Python API
|
||||
import intera_interface
|
||||
|
||||
# initialize our ROS node, registering it with the Master
|
||||
rospy.init_node('Hello_Sawyer')
|
||||
|
||||
# create an instance of intera_interface's Limb class
|
||||
limb = intera_interface.Limb('right')
|
||||
|
||||
# get the right limb's current joint angles
|
||||
angles = limb.joint_angles()
|
||||
|
||||
# print the current joint angles as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
# move to neutral pose
|
||||
limb.move_to_neutral()
|
||||
|
||||
# get the right limb's current joint angles now that it is in neutral
|
||||
angles = limb.joint_angles()
|
||||
|
||||
# print the current joint angles again as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
|
||||
# reassign new joint angles (all zeros) which we will later command to the limb
|
||||
angles['right_j0']=0.0
|
||||
angles['right_j1']=0.0
|
||||
angles['right_j2']=0.0
|
||||
angles['right_j3']=0.0
|
||||
angles['right_j4']=0.0
|
||||
angles['right_j5']=0.0
|
||||
angles['right_j6']=0.0
|
||||
|
||||
# print the joint angle command as valid json
|
||||
print json.dumps(angles)
|
||||
|
||||
# move the right arm to those joint angles
|
||||
limb.move_to_joint_positions(angles)
|
||||
|
||||
# Sawyer wants to say hello, let's wave the arm
|
||||
|
||||
# store the first wave position
|
||||
wave_1 = {'right_j6': -1.5126, 'right_j5': -0.3438, 'right_j4': 1.5126, 'right_j3': -1.3833, 'right_j2': 0.03726, 'right_j1': 0.3526, 'right_j0': -0.4259}
|
||||
|
||||
# store the second wave position
|
||||
wave_2 = {'right_j6': -1.5101, 'right_j5': -0.3806, 'right_j4': 1.5103, 'right_j3': -1.4038, 'right_j2': -0.2609, 'right_j1': 0.3940, 'right_j0': -0.4281}
|
||||
|
||||
|
||||
# wave three times
|
||||
for _move in range(3):
|
||||
limb.move_to_joint_positions(wave_1)
|
||||
rospy.sleep(0.5)
|
||||
limb.move_to_joint_positions(wave_2)
|
||||
rospy.sleep(0.5)
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
import json
|
||||
import requests
|
||||
import re
|
||||
from io import BytesIO
|
||||
|
||||
# Used to call Python 2.7 from 3.6
|
||||
import subprocess
|
||||
|
||||
from aiohttp import web
|
||||
from botbuilder.core import (BotFrameworkAdapter, BotFrameworkAdapterSettings, TurnContext)
|
||||
from botbuilder.schema import (Activity, ActivityTypes)
|
||||
|
||||
|
||||
# Settings
|
||||
BOT_APP_ID = ''
|
||||
BOT_APP_PASSWORD = ''
|
||||
SETTINGS = BotFrameworkAdapterSettings(BOT_APP_ID, BOT_APP_PASSWORD)
|
||||
ADAPTER = BotFrameworkAdapter(SETTINGS)
|
||||
|
||||
LUIS_APP_ID = '<your luis application id>'
|
||||
LUIS_SUBSCRIPTION_KEY = '<your luis subscription key>'
|
||||
|
||||
COMPUTER_VISION_ANALYZE_URL = "https://westus.api.cognitive.microsoft.com/vision/v2.0/analyze"
|
||||
COMPUTER_VISION_SUBSCRIPTION_KEY = "<your computer vision subscription key>"
|
||||
|
||||
SIM_API_HOST = 'http://localhost:5000'
|
||||
|
||||
class ComputerVisionApiService:
|
||||
@staticmethod
|
||||
def analyze_image(image_url):
|
||||
# Request headers and parameters
|
||||
|
||||
|
||||
# Get image bytes content
|
||||
|
||||
|
||||
try:
|
||||
return None # Replace with implementation
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
|
||||
class LuisResponse():
|
||||
def __init__(self, intent, entity_value = None, entity_type = None):
|
||||
self.intent = intent
|
||||
self.entity_value = entity_value
|
||||
self.entity_type = entity_type
|
||||
|
||||
class LuisApiService:
|
||||
@staticmethod
|
||||
def post_utterance(message):
|
||||
# Request headers and parameters
|
||||
|
||||
|
||||
try:
|
||||
return None # Replace with implementation
|
||||
except Exception as e:
|
||||
print("[Errno {0}] {1}".format(e.errno, e.strerror))
|
||||
|
||||
|
||||
class BotRequestHandler:
|
||||
async def create_reply_activity(request_activity, text) -> Activity:
|
||||
return Activity(
|
||||
type=ActivityTypes.message,
|
||||
channel_id=request_activity.channel_id,
|
||||
conversation=request_activity.conversation,
|
||||
recipient=request_activity.from_property,
|
||||
from_property=request_activity.recipient,
|
||||
text=text,
|
||||
service_url=request_activity.service_url)
|
||||
|
||||
async def handle_message(context: TurnContext) -> web.Response:
|
||||
activity = context.activity
|
||||
if activity.text:
|
||||
luis_result = LuisResponse('None')
|
||||
|
||||
response = await BotRequestHandler.create_reply_activity(activity, f'Top Intent: {luis_result.intent}.')
|
||||
await context.send_activity(response)
|
||||
|
||||
if luis_result.intent == 'MoveArm':
|
||||
BotCommandHandler.move_arm()
|
||||
else:
|
||||
response = await BotRequestHandler.create_reply_activity(activity, 'Please provide a valid instruction')
|
||||
await context.send_activity(response)
|
||||
else:
|
||||
await BotRequestHandler.process_image(activity, context)
|
||||
|
||||
return web.Response(status=202)
|
||||
|
||||
async def handle_conversation_update(context: TurnContext) -> web.Response:
|
||||
if context.activity.members_added[0].id != context.activity.recipient.id:
|
||||
response = await BotRequestHandler.create_reply_activity(context.activity, 'Welcome to Sawyer Robot!')
|
||||
await context.send_activity(response)
|
||||
return web.Response(status=200)
|
||||
|
||||
async def unhandled_activity() -> web.Response:
|
||||
return web.Response(status=404)
|
||||
|
||||
async def process_image(activity: Activity, context: TurnContext):
|
||||
print("Process image") # Replace with implementation
|
||||
|
||||
def get_image_url(attachments):
|
||||
p = re.compile('^image/(jpg|jpeg|png|gif)$')
|
||||
for attachment in attachments:
|
||||
rs = p.match(attachment.content_type)
|
||||
if rs:
|
||||
return attachment.content_url
|
||||
return None
|
||||
|
||||
|
||||
@staticmethod
|
||||
async def request_handler(context: TurnContext) -> web.Response:
|
||||
if context.activity.type == 'message':
|
||||
return await BotRequestHandler.handle_message(context)
|
||||
elif context.activity.type == 'conversationUpdate':
|
||||
return await BotRequestHandler.handle_conversation_update(context)
|
||||
else:
|
||||
return await BotRequestHandler.unhandled_activity()
|
||||
|
||||
async def messages(req: web.web_request) -> web.Response:
|
||||
body = await req.json()
|
||||
activity = Activity().deserialize(body)
|
||||
auth_header = req.headers['Authorization'] if 'Authorization' in req.headers else ''
|
||||
try:
|
||||
return await ADAPTER.process_activity(activity, auth_header, BotRequestHandler.request_handler)
|
||||
except Exception as e:
|
||||
raise e
|
||||
|
||||
class BotCommandHandler:
|
||||
def move_arm():
|
||||
print('Moving arm... do something cool')
|
||||
# launch your python2 script using bash
|
||||
python2_command = "python2.7 bot-wave-arm-node.py"
|
||||
|
||||
process = subprocess.Popen(python2_command.split(), stdout=subprocess.PIPE)
|
||||
output, error = process.communicate() # receive output from the python2 script
|
||||
|
||||
print('done moving arm . . .')
|
||||
print('returncode: ' + str(process.returncode))
|
||||
print('output: ' + output.decode("utf-8"))
|
||||
|
||||
def show_stats():
|
||||
print("Replace with your code")
|
||||
|
||||
def move_cube(color):
|
||||
print("Replace with your code")
|
||||
|
||||
def move_grippers(action):
|
||||
print("Replace with your code")
|
||||
|
||||
|
||||
|
||||
app = web.Application()
|
||||
app.router.add_post('/api/messages', BotRequestHandler.messages)
|
||||
|
||||
try:
|
||||
web.run_app(app, host='localhost', port=9000)
|
||||
print('Started http server on localhost:9000')
|
||||
except Exception as e:
|
||||
raise e
|
||||
|
|
@ -0,0 +1,201 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(sawyer_ik_5d)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
kdl_parser
|
||||
moveit_msgs
|
||||
roscpp
|
||||
)
|
||||
|
||||
find_package(orocos_kdl)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs # Or other packages containing msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES sawyer_ik_5d
|
||||
# CATKIN_DEPENDS orocos_kdl roscpp
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/sawyer_ik_5d.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
add_executable(${PROJECT_NAME}_node src/sawyer_ik_5d_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(${PROJECT_NAME}_node
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_sawyer_ik_5d.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>sawyer_ik_5d</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The sawyer_ik_5d package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="pblasco@plainconcepts.com">Pablo Inigo Blasco</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>copyright</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/sawyer_ik_5d</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>orocos_kdl</build_depend>
|
||||
<build_depend>kdl_parser</build_depend>
|
||||
<build_depend>moveit_msgs</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_export_depend>orocos_kdl</build_export_depend>
|
||||
<build_export_depend>kdl_parser</build_export_depend>
|
||||
<build_export_depend>moveit_msgs</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<exec_depend>orocos_kdl</exec_depend>
|
||||
<exec_depend>kdl_parser</exec_depend>
|
||||
<exec_depend>moveit_msgs</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,619 @@
|
|||
#include <urdf/model.h>
|
||||
#include <kdl/chainiksolverpos_lma.hpp>
|
||||
#include <kdl/chainfksolverpos_recursive.hpp>
|
||||
#include <kdl_parser/kdl_parser.hpp>
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/GetPositionIK.h>
|
||||
|
||||
using namespace KDL;
|
||||
using namespace Eigen;
|
||||
using namespace ros;
|
||||
using namespace std;
|
||||
|
||||
#define SAWYER_JOINT_COUNT 6
|
||||
|
||||
ChainIkSolverPos_LMA *ikSolverPos;
|
||||
ChainFkSolverPos_recursive *fkSolverPos;
|
||||
Tree tree;
|
||||
Chain chain;
|
||||
Eigen::Matrix<double,6/*pose params*/,1> L;
|
||||
vector<string> joint_names;
|
||||
JntArray q_current(SAWYER_JOINT_COUNT);
|
||||
|
||||
ros::ServiceServer ikservice;
|
||||
bool initialized= false;
|
||||
|
||||
void getParameters()
|
||||
{
|
||||
NodeHandle nh;
|
||||
////////////////////////////////////////////////////////////
|
||||
// Get robot description
|
||||
////////////////////////////////////////////////////////////
|
||||
string description;
|
||||
if(!nh.getParam("/robot_description", description))
|
||||
{
|
||||
ROS_ERROR("Parameter not set: /robot_description");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Create robot model
|
||||
////////////////////////////////////////////////////////////
|
||||
urdf::Model model;
|
||||
if(!model.initString(description))
|
||||
{
|
||||
ROS_ERROR("Could not initialize robot model");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Get tip link name and verify it exists in the model
|
||||
////////////////////////////////////////////////////////////
|
||||
string tip_link_name;
|
||||
if(!nh.getParam("tip_link_name", tip_link_name))
|
||||
{
|
||||
tip_link_name = "right_hand_camera_optical";
|
||||
ROS_WARN("Parameter not set: %s/tip_link_name, default value -> %s", nh.getNamespace().c_str(), tip_link_name.c_str());
|
||||
//exit(-1);
|
||||
}
|
||||
if(model.links_.find(tip_link_name) == model.links_.end())
|
||||
{
|
||||
ROS_ERROR("The link specified in %s/tip_link_name does not exist", nh.getNamespace().c_str());
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Get revolute joints
|
||||
////////////////////////////////////////////////////////////
|
||||
vector<boost::shared_ptr<urdf::Joint> > model_joints;
|
||||
|
||||
for(boost::shared_ptr<const urdf::Link> current_link = model.getLink(tip_link_name); current_link->parent_joint != NULL; current_link = current_link->getParent())
|
||||
{
|
||||
if(current_link->parent_joint->type == urdf::Joint::REVOLUTE)
|
||||
{
|
||||
model_joints.insert(model_joints.begin(), current_link->parent_joint);
|
||||
}
|
||||
}
|
||||
|
||||
if(model_joints.size() != SAWYER_JOINT_COUNT)
|
||||
{
|
||||
ROS_ERROR("The robot model must have %d revolute joints, found %d", SAWYER_JOINT_COUNT, (int)model_joints.size());
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Configure servo limits
|
||||
////////////////////////////////////////////////////////////
|
||||
// for(int joint_i = 0; joint_i < SAWYER_JOINT_COUNT; joint_i++)
|
||||
// {
|
||||
// float lowerLimit = model_joints[joint_i]->limits->lower;
|
||||
// float upperLimit = model_joints[joint_i]->limits->upper;
|
||||
// }
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Store joint names
|
||||
////////////////////////////////////////////////////////////
|
||||
for(int joint_i = 0; joint_i < SAWYER_JOINT_COUNT; joint_i++)
|
||||
{
|
||||
joint_names.push_back(model_joints[joint_i]->name);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Create KDL tree
|
||||
////////////////////////////////////////////////////////////
|
||||
if(!kdl_parser::treeFromString(description, tree))
|
||||
{
|
||||
ROS_ERROR("Failed to construct kdl tree");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
if(!tree.getChain(model.getRoot()->name, tip_link_name, chain))
|
||||
{
|
||||
ROS_ERROR("Failed to make chain from tree");
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
void initialize_ik()
|
||||
{
|
||||
getParameters();
|
||||
|
||||
ROS_INFO("creating solvers");
|
||||
// Create KDL solvers
|
||||
L(0) = 1; L(1) = 1; L(2) = 1; L(3) = 1; L(4) = 1; L(5) = 0 ;
|
||||
ikSolverPos = new KDL::ChainIkSolverPos_LMA(chain, L);
|
||||
fkSolverPos = new KDL::ChainFkSolverPos_recursive(chain);
|
||||
}
|
||||
|
||||
bool ik(moveit_msgs::GetPositionIK::Request& request, moveit_msgs::GetPositionIK::Response& response)
|
||||
{
|
||||
ROS_INFO("ik request");
|
||||
if(!initialized)
|
||||
{
|
||||
ROS_INFO("initializing");
|
||||
initialize_ik();
|
||||
ROS_INFO("initialized");
|
||||
}
|
||||
|
||||
ROS_INFO("reading inputs");
|
||||
for(int i=0;i<SAWYER_JOINT_COUNT;i++)
|
||||
{
|
||||
q_current(i) = request.ik_request.robot_state.joint_state.position[i];
|
||||
}
|
||||
|
||||
const geometry_msgs::Point& p = request.ik_request.pose_stamped.pose.position;
|
||||
const geometry_msgs::Quaternion& q = request.ik_request.pose_stamped.pose.orientation;
|
||||
|
||||
Frame target_frame (Rotation::Quaternion(q.x,q.y,q.z,q.w),Vector(p.x,p.y,p.z));
|
||||
|
||||
ROS_INFO("computing ik, target %lf %lf %lf", target_frame.p[0],target_frame.p[1],target_frame.p[2]);
|
||||
JntArray q_res;
|
||||
int result = ikSolverPos->CartToJnt(q_current, target_frame,q_res);
|
||||
ROS_INFO("IK RESULT: %d", result);
|
||||
//0 if successful, -1 the gradient of $ E $ towards the joints is to small, -2 if joint position increments are to small, -3 if number of iterations is exceeded.
|
||||
|
||||
sensor_msgs::JointState& respjoints= response.solution.joint_state;
|
||||
|
||||
respjoints.name= request.ik_request.robot_state.joint_state.name;
|
||||
|
||||
for(int i=0;i<SAWYER_JOINT_COUNT;i++)
|
||||
respjoints.position.push_back(q_res(i));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// Init ROS node
|
||||
ros::init(argc, argv, "sawyer_ik_5d");
|
||||
|
||||
// Get node handle
|
||||
NodeHandle nh("~");
|
||||
|
||||
ikservice = nh.advertiseService("ik", ik);
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
/*
|
||||
moveit_msgs/PositionIKRequest ik_request
|
||||
string group_name
|
||||
moveit_msgs/RobotState robot_state
|
||||
sensor_msgs/JointState joint_state
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] name
|
||||
float64[] position
|
||||
float64[] velocity
|
||||
float64[] effort
|
||||
sensor_msgs/MultiDOFJointState multi_dof_joint_state
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] joint_names
|
||||
geometry_msgs/Transform[] transforms
|
||||
geometry_msgs/Vector3 translation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion rotation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
geometry_msgs/Twist[] twist
|
||||
geometry_msgs/Vector3 linear
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Vector3 angular
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Wrench[] wrench
|
||||
geometry_msgs/Vector3 force
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Vector3 torque
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
moveit_msgs/AttachedCollisionObject[] attached_collision_objects
|
||||
string link_name
|
||||
moveit_msgs/CollisionObject object
|
||||
byte ADD=0
|
||||
byte REMOVE=1
|
||||
byte APPEND=2
|
||||
byte MOVE=3
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string id
|
||||
object_recognition_msgs/ObjectType type
|
||||
string key
|
||||
string db
|
||||
shape_msgs/SolidPrimitive[] primitives
|
||||
uint8 BOX=1
|
||||
uint8 SPHERE=2
|
||||
uint8 CYLINDER=3
|
||||
uint8 CONE=4
|
||||
uint8 BOX_X=0
|
||||
uint8 BOX_Y=1
|
||||
uint8 BOX_Z=2
|
||||
uint8 SPHERE_RADIUS=0
|
||||
uint8 CYLINDER_HEIGHT=0
|
||||
uint8 CYLINDER_RADIUS=1
|
||||
uint8 CONE_HEIGHT=0
|
||||
uint8 CONE_RADIUS=1
|
||||
uint8 type
|
||||
float64[] dimensions
|
||||
geometry_msgs/Pose[] primitive_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
shape_msgs/Mesh[] meshes
|
||||
shape_msgs/MeshTriangle[] triangles
|
||||
uint32[3] vertex_indices
|
||||
geometry_msgs/Point[] vertices
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Pose[] mesh_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
shape_msgs/Plane[] planes
|
||||
float64[4] coef
|
||||
geometry_msgs/Pose[] plane_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
byte operation
|
||||
string[] touch_links
|
||||
trajectory_msgs/JointTrajectory detach_posture
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] joint_names
|
||||
trajectory_msgs/JointTrajectoryPoint[] points
|
||||
float64[] positions
|
||||
float64[] velocities
|
||||
float64[] accelerations
|
||||
float64[] effort
|
||||
duration time_from_start
|
||||
float64 weight
|
||||
bool is_diff
|
||||
moveit_msgs/Constraints constraints
|
||||
string name
|
||||
moveit_msgs/JointConstraint[] joint_constraints
|
||||
string joint_name
|
||||
float64 position
|
||||
float64 tolerance_above
|
||||
float64 tolerance_below
|
||||
float64 weight
|
||||
moveit_msgs/PositionConstraint[] position_constraints
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string link_name
|
||||
geometry_msgs/Vector3 target_point_offset
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
moveit_msgs/BoundingVolume constraint_region
|
||||
shape_msgs/SolidPrimitive[] primitives
|
||||
uint8 BOX=1
|
||||
uint8 SPHERE=2
|
||||
uint8 CYLINDER=3
|
||||
uint8 CONE=4
|
||||
uint8 BOX_X=0
|
||||
uint8 BOX_Y=1
|
||||
uint8 BOX_Z=2
|
||||
uint8 SPHERE_RADIUS=0
|
||||
uint8 CYLINDER_HEIGHT=0
|
||||
uint8 CYLINDER_RADIUS=1
|
||||
uint8 CONE_HEIGHT=0
|
||||
uint8 CONE_RADIUS=1
|
||||
uint8 type
|
||||
float64[] dimensions
|
||||
geometry_msgs/Pose[] primitive_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
shape_msgs/Mesh[] meshes
|
||||
shape_msgs/MeshTriangle[] triangles
|
||||
uint32[3] vertex_indices
|
||||
geometry_msgs/Point[] vertices
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Pose[] mesh_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
float64 weight
|
||||
moveit_msgs/OrientationConstraint[] orientation_constraints
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
string link_name
|
||||
float64 absolute_x_axis_tolerance
|
||||
float64 absolute_y_axis_tolerance
|
||||
float64 absolute_z_axis_tolerance
|
||||
float64 weight
|
||||
moveit_msgs/VisibilityConstraint[] visibility_constraints
|
||||
uint8 SENSOR_Z=0
|
||||
uint8 SENSOR_Y=1
|
||||
uint8 SENSOR_X=2
|
||||
float64 target_radius
|
||||
geometry_msgs/PoseStamped target_pose
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
geometry_msgs/Pose pose
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
int32 cone_sides
|
||||
geometry_msgs/PoseStamped sensor_pose
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
geometry_msgs/Pose pose
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
float64 max_view_angle
|
||||
float64 max_range_angle
|
||||
uint8 sensor_view_direction
|
||||
float64 weight
|
||||
bool avoid_collisions
|
||||
string ik_link_name
|
||||
geometry_msgs/PoseStamped pose_stamped
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
geometry_msgs/Pose pose
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
string[] ik_link_names
|
||||
geometry_msgs/PoseStamped[] pose_stamped_vector
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
geometry_msgs/Pose pose
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
duration timeout
|
||||
int32 attempts
|
||||
---
|
||||
moveit_msgs/RobotState solution
|
||||
sensor_msgs/JointState joint_state
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] name
|
||||
float64[] position
|
||||
float64[] velocity
|
||||
float64[] effort
|
||||
sensor_msgs/MultiDOFJointState multi_dof_joint_state
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] joint_names
|
||||
geometry_msgs/Transform[] transforms
|
||||
geometry_msgs/Vector3 translation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion rotation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
geometry_msgs/Twist[] twist
|
||||
geometry_msgs/Vector3 linear
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Vector3 angular
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Wrench[] wrench
|
||||
geometry_msgs/Vector3 force
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Vector3 torque
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
moveit_msgs/AttachedCollisionObject[] attached_collision_objects
|
||||
string link_name
|
||||
moveit_msgs/CollisionObject object
|
||||
byte ADD=0
|
||||
byte REMOVE=1
|
||||
byte APPEND=2
|
||||
byte MOVE=3
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string id
|
||||
object_recognition_msgs/ObjectType type
|
||||
string key
|
||||
string db
|
||||
shape_msgs/SolidPrimitive[] primitives
|
||||
uint8 BOX=1
|
||||
uint8 SPHERE=2
|
||||
uint8 CYLINDER=3
|
||||
uint8 CONE=4
|
||||
uint8 BOX_X=0
|
||||
uint8 BOX_Y=1
|
||||
uint8 BOX_Z=2
|
||||
uint8 SPHERE_RADIUS=0
|
||||
uint8 CYLINDER_HEIGHT=0
|
||||
uint8 CYLINDER_RADIUS=1
|
||||
uint8 CONE_HEIGHT=0
|
||||
uint8 CONE_RADIUS=1
|
||||
uint8 type
|
||||
float64[] dimensions
|
||||
geometry_msgs/Pose[] primitive_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
shape_msgs/Mesh[] meshes
|
||||
shape_msgs/MeshTriangle[] triangles
|
||||
uint32[3] vertex_indices
|
||||
geometry_msgs/Point[] vertices
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Pose[] mesh_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
shape_msgs/Plane[] planes
|
||||
float64[4] coef
|
||||
geometry_msgs/Pose[] plane_poses
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
byte operation
|
||||
string[] touch_links
|
||||
trajectory_msgs/JointTrajectory detach_posture
|
||||
std_msgs/Header header
|
||||
uint32 seq
|
||||
time stamp
|
||||
string frame_id
|
||||
string[] joint_names
|
||||
trajectory_msgs/JointTrajectoryPoint[] points
|
||||
float64[] positions
|
||||
float64[] velocities
|
||||
float64[] accelerations
|
||||
float64[] effort
|
||||
duration time_from_start
|
||||
float64 weight
|
||||
bool is_diff
|
||||
moveit_msgs/MoveItErrorCodes error_code
|
||||
int32 SUCCESS=1
|
||||
int32 FAILURE=99999
|
||||
int32 PLANNING_FAILED=-1
|
||||
int32 INVALID_MOTION_PLAN=-2
|
||||
int32 MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE=-3
|
||||
int32 CONTROL_FAILED=-4
|
||||
int32 UNABLE_TO_AQUIRE_SENSOR_DATA=-5
|
||||
int32 TIMED_OUT=-6
|
||||
int32 PREEMPTED=-7
|
||||
int32 START_STATE_IN_COLLISION=-10
|
||||
int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-11
|
||||
int32 GOAL_IN_COLLISION=-12
|
||||
int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-13
|
||||
int32 GOAL_CONSTRAINTS_VIOLATED=-14
|
||||
int32 INVALID_GROUP_NAME=-15
|
||||
int32 INVALID_GOAL_CONSTRAINTS=-16
|
||||
int32 INVALID_ROBOT_STATE=-17
|
||||
int32 INVALID_LINK_NAME=-18
|
||||
int32 INVALID_OBJECT_NAME=-19
|
||||
int32 FRAME_TRANSFORM_FAILURE=-21
|
||||
int32 COLLISION_CHECKING_UNAVAILABLE=-22
|
||||
int32 ROBOT_STATE_STALE=-23
|
||||
int32 SENSOR_INFO_STALE=-24
|
||||
int32 NO_IK_SOLUTION=-31
|
||||
int32 val
|
||||
|
||||
*/
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(sorting_demo)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
cv_bridge
|
||||
rospy
|
||||
actionlib
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
control_msgs
|
||||
trajectory_msgs
|
||||
dynamic_reconfigure
|
||||
intera_core_msgs
|
||||
intera_motion_msgs
|
||||
gazebo_msgs
|
||||
)
|
||||
|
||||
catkin_python_setup()
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
rospy
|
||||
intera_core_msgs
|
||||
gazebo_msgs
|
||||
)
|
||||
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
install(PROGRAMS
|
||||
src/sorting_demo/program.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
|
||||
foreach(dir launch models)
|
||||
install(DIRECTORY ${dir}/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||
endforeach(dir)
|
||||
|
|
@ -0,0 +1,546 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
- /Path1
|
||||
- /Path2
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 573
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base:
|
||||
Value: true
|
||||
block0::block:
|
||||
Value: true
|
||||
block1::block:
|
||||
Value: true
|
||||
block2::block:
|
||||
Value: true
|
||||
block3::block:
|
||||
Value: true
|
||||
block4::block:
|
||||
Value: true
|
||||
block5::block:
|
||||
Value: true
|
||||
block6::block:
|
||||
Value: true
|
||||
block7::block:
|
||||
Value: true
|
||||
block8::block:
|
||||
Value: true
|
||||
controller_box:
|
||||
Value: true
|
||||
head:
|
||||
Value: true
|
||||
head_camera:
|
||||
Value: true
|
||||
head_camera_optical:
|
||||
Value: true
|
||||
pedestal:
|
||||
Value: true
|
||||
pedestal_feet:
|
||||
Value: true
|
||||
right_arm_base_link:
|
||||
Value: true
|
||||
right_arm_itb:
|
||||
Value: true
|
||||
right_connector_plate_base:
|
||||
Value: true
|
||||
right_connector_plate_mount:
|
||||
Value: true
|
||||
right_electric_gripper_base:
|
||||
Value: true
|
||||
right_gripper_base:
|
||||
Value: true
|
||||
right_gripper_l_finger:
|
||||
Value: true
|
||||
right_gripper_l_finger_tip:
|
||||
Value: true
|
||||
right_gripper_r_finger:
|
||||
Value: true
|
||||
right_gripper_r_finger_tip:
|
||||
Value: true
|
||||
right_gripper_tip:
|
||||
Value: true
|
||||
right_hand:
|
||||
Value: true
|
||||
right_hand_camera:
|
||||
Value: true
|
||||
right_hand_camera_optical:
|
||||
Value: true
|
||||
right_l0:
|
||||
Value: true
|
||||
right_l1:
|
||||
Value: true
|
||||
right_l1_2:
|
||||
Value: true
|
||||
right_l2:
|
||||
Value: true
|
||||
right_l2_2:
|
||||
Value: true
|
||||
right_l3:
|
||||
Value: true
|
||||
right_l4:
|
||||
Value: true
|
||||
right_l4_2:
|
||||
Value: true
|
||||
right_l5:
|
||||
Value: true
|
||||
right_l6:
|
||||
Value: true
|
||||
right_torso_itb:
|
||||
Value: true
|
||||
right_wrist:
|
||||
Value: true
|
||||
screen:
|
||||
Value: true
|
||||
torso:
|
||||
Value: true
|
||||
tray0::tray_base:
|
||||
Value: true
|
||||
tray1::tray_base:
|
||||
Value: true
|
||||
tray2::tray_base:
|
||||
Value: true
|
||||
world:
|
||||
Value: true
|
||||
Marker Scale: 0.25
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
world:
|
||||
base:
|
||||
controller_box:
|
||||
{}
|
||||
pedestal:
|
||||
{}
|
||||
pedestal_feet:
|
||||
{}
|
||||
right_arm_base_link:
|
||||
right_l0:
|
||||
head:
|
||||
head_camera:
|
||||
head_camera_optical:
|
||||
{}
|
||||
screen:
|
||||
{}
|
||||
right_l1:
|
||||
right_l1_2:
|
||||
{}
|
||||
right_l2:
|
||||
right_l2_2:
|
||||
{}
|
||||
right_l3:
|
||||
right_l4:
|
||||
right_arm_itb:
|
||||
{}
|
||||
right_l4_2:
|
||||
{}
|
||||
right_l5:
|
||||
right_hand_camera:
|
||||
right_hand_camera_optical:
|
||||
{}
|
||||
right_l6:
|
||||
right_hand:
|
||||
right_gripper_base:
|
||||
right_connector_plate_base:
|
||||
right_connector_plate_mount:
|
||||
right_electric_gripper_base:
|
||||
right_gripper_l_finger:
|
||||
right_gripper_l_finger_tip:
|
||||
{}
|
||||
right_gripper_r_finger:
|
||||
right_gripper_r_finger_tip:
|
||||
{}
|
||||
right_gripper_tip:
|
||||
{}
|
||||
right_wrist:
|
||||
{}
|
||||
right_torso_itb:
|
||||
{}
|
||||
torso:
|
||||
{}
|
||||
block0::block:
|
||||
{}
|
||||
block1::block:
|
||||
{}
|
||||
block2::block:
|
||||
{}
|
||||
block3::block:
|
||||
{}
|
||||
block4::block:
|
||||
{}
|
||||
block5::block:
|
||||
{}
|
||||
block6::block:
|
||||
{}
|
||||
block7::block:
|
||||
{}
|
||||
block8::block:
|
||||
{}
|
||||
tray0::tray_base:
|
||||
{}
|
||||
tray1::tray_base:
|
||||
{}
|
||||
tray2::tray_base:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /io/internal_camera/right_hand_camera/image_rect
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Line Style: Lines
|
||||
Line Width: 1
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Topic: /reelrbtx_follow_controller/reel_mouth_path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 0; 255
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Topic: /reelrbtx_follow_controller/reel_base_path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
controller_box:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
head:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
head_camera:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
head_camera_optical:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
pedestal:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
pedestal_feet:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_arm_base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_arm_itb:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_connector_plate_base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_connector_plate_mount:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_electric_gripper_base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_gripper_base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_gripper_l_finger:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_gripper_l_finger_tip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_gripper_r_finger:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_gripper_r_finger_tip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_gripper_tip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_hand:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_hand_camera:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_hand_camera_optical:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_l0:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l1_2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l2_2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l4_2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_l6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_torso_itb:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
right_wrist:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
screen:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
torso:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 2.20854378
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.463854343
|
||||
Y: 0.0451556407
|
||||
Z: 0.0798519328
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.0500000007
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.304797798
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 5.46816921
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000002f400000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000027e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d3000001300000000000000000fb0000000a0049006d00610067006501000002ac000001120000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003300000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1855
|
||||
X: 65
|
||||
Y: 24
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<launch>
|
||||
|
||||
<!-- We resume the logic in empty_world.launch, changing the name of the world to be launched -->
|
||||
<include file="$(find sawyer_gazebo)/launch/sawyer_world.launch">
|
||||
<arg name="electric_gripper" value="true"/>
|
||||
</include>
|
||||
|
||||
<!-- Start the Sawyer pick and place demo -->
|
||||
<node pkg="sorting_demo" type="program.py" name="sorting_demo" />
|
||||
|
||||
<node pkg="sawyer_ik_5d" type="sawyer_ik_5d_node" name="sawyer_ik_5d_node" output="screen"/>
|
||||
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find sorting_demo)/config/rviz.rviz"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
<robot xmlns:xacro="http://ros.org/wiki/xacro"
|
||||
name="block">
|
||||
|
||||
<xacro:arg name="material" default="Gazebo/Black" />
|
||||
<xacro:arg name="edge_length" default="0.04" />
|
||||
|
||||
<xacro:property name="block_density" value="3900" />
|
||||
|
||||
<link name="block">
|
||||
<inertial>
|
||||
<mass value="${block_density * $(arg edge_length)**3}" />
|
||||
<inertia ixx="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" iyz="0.0"
|
||||
izz="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="$(arg edge_length) $(arg edge_length) $(arg edge_length)" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="$(arg edge_length) $(arg edge_length) $(arg edge_length)" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<gazebo reference="block">
|
||||
<material>$(arg material)</material>
|
||||
<mu1>10000</mu1>
|
||||
<mu2>10000</mu2>
|
||||
|
||||
<collision>
|
||||
<surface>
|
||||
<friction>
|
||||
<torsional>
|
||||
<coefficient>0.456</coefficient>
|
||||
<use_patch_radius>true</use_patch_radius>
|
||||
<patch_radius>0.05</patch_radius>
|
||||
</torsional>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</gazebo>
|
||||
|
||||
|
||||
</robot>
|
||||
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 36 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 21 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 44 KiB |
|
|
@ -0,0 +1,307 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<authoring_tool>SketchUp 14.1.1282</authoring_tool>
|
||||
</contributor>
|
||||
<created>2014-12-10T16:57:23Z</created>
|
||||
<modified>2014-12-10T16:57:23Z</modified>
|
||||
<unit meter="0.0254" name="inch" />
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="ID1">
|
||||
<node name="SketchUp">
|
||||
<instance_geometry url="#ID38">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material2" target="#ID39">
|
||||
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<node id="ID2" name="Top_Square">
|
||||
<matrix>1 0 0 1.024181e-014 0 1 0 8.881784e-015 0 0 1 29 0 0 0 1</matrix>
|
||||
<instance_node url="#ID3" />
|
||||
</node>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<library_nodes>
|
||||
<node id="ID3" name="Top_Square">
|
||||
<instance_geometry url="#ID4">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material2" target="#ID5">
|
||||
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
|
||||
</instance_material>
|
||||
<instance_material symbol="Material3" target="#ID14">
|
||||
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<instance_geometry url="#ID22">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material2" target="#ID14">
|
||||
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<instance_geometry url="#ID30">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material2" target="#ID14">
|
||||
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</library_nodes>
|
||||
<library_geometries>
|
||||
<geometry id="ID4">
|
||||
<mesh>
|
||||
<source id="ID10">
|
||||
<float_array id="ID19" count="96">18 18 0 18 -18 1.5 18 -18 0 18 18 1.5 18 18 1.5 18 18 0 18 -18 1.5 18 -18 0 -18 18 1.5 18 18 0 -18 18 0 18 18 1.5 18 18 1.5 -18 18 1.5 18 18 0 -18 18 0 18 -18 1.5 -18 -18 0 18 -18 0 -18 -18 1.5 -18 -18 1.5 18 -18 1.5 -18 -18 0 18 -18 0 -18 18 1.5 -18 -18 0 -18 -18 1.5 -18 18 0 -18 18 0 -18 18 1.5 -18 -18 0 -18 -18 1.5</float_array>
|
||||
<technique_common>
|
||||
<accessor count="32" source="#ID19" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID11">
|
||||
<float_array id="ID20" count="96">1 0 0 1 0 0 1 0 0 1 0 0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0</float_array>
|
||||
<technique_common>
|
||||
<accessor count="32" source="#ID20" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID13">
|
||||
<float_array id="ID21" count="64">0.15 0 -0.15 1.5 -0.15 0 0.15 1.5 0.75 0.0625 0.75 0 -0.75 0.0625 -0.75 0 0.15 1.5 -0.15 0 0.15 0 -0.15 1.5 -0.75 0.0625 0.75 0.0625 -0.75 0 0.75 0 0.15 1.5 -0.15 0 0.15 0 -0.15 1.5 -0.75 0.0625 0.75 0.0625 -0.75 0 0.75 0 -0.15 1.5 0.15 0 0.15 1.5 -0.15 0 -0.75 0 -0.75 0.0625 0.75 0 0.75 0.0625</float_array>
|
||||
<technique_common>
|
||||
<accessor count="32" source="#ID21" stride="2">
|
||||
<param name="S" type="float" />
|
||||
<param name="T" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID12">
|
||||
<input semantic="POSITION" source="#ID10" />
|
||||
<input semantic="NORMAL" source="#ID11" />
|
||||
</vertices>
|
||||
<triangles count="8" material="Material2">
|
||||
<input offset="0" semantic="VERTEX" source="#ID12" />
|
||||
<input offset="1" semantic="TEXCOORD" source="#ID13" />
|
||||
<p>0 0 1 1 2 2 1 1 0 0 3 3 8 8 9 9 10 10 9 9 8 8 11 11 16 16 17 17 18 18 17 17 16 16 19 19 24 24 25 25 26 26 25 25 24 24 27 27</p>
|
||||
</triangles>
|
||||
<triangles count="8" material="Material3">
|
||||
<input offset="0" semantic="VERTEX" source="#ID12" />
|
||||
<input offset="1" semantic="TEXCOORD" source="#ID13" />
|
||||
<p>4 4 5 5 6 6 7 7 6 6 5 5 12 12 13 13 14 14 15 15 14 14 13 13 20 20 21 21 22 22 23 23 22 22 21 21 28 28 29 29 30 30 31 31 30 30 29 29</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<geometry id="ID22">
|
||||
<mesh>
|
||||
<source id="ID23">
|
||||
<float_array id="ID27" count="24">18 -18 1.5 -18 18 1.5 -18 -18 1.5 18 18 1.5 18 18 1.5 18 -18 1.5 -18 18 1.5 -18 -18 1.5</float_array>
|
||||
<technique_common>
|
||||
<accessor count="8" source="#ID27" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID24">
|
||||
<float_array id="ID28" count="24">0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1</float_array>
|
||||
<technique_common>
|
||||
<accessor count="8" source="#ID28" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID26">
|
||||
<float_array id="ID29" count="8">0.75 -0.75 -0.75 0.75 -0.75 -0.75 0.75 0.75</float_array>
|
||||
<technique_common>
|
||||
<accessor count="4" source="#ID29" stride="2">
|
||||
<param name="S" type="float" />
|
||||
<param name="T" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID25">
|
||||
<input semantic="POSITION" source="#ID23" />
|
||||
<input semantic="NORMAL" source="#ID24" />
|
||||
</vertices>
|
||||
<triangles count="4" material="Material2">
|
||||
<input offset="0" semantic="VERTEX" source="#ID25" />
|
||||
<input offset="1" semantic="TEXCOORD" source="#ID26" />
|
||||
<p>0 0 1 1 2 2 1 1 0 0 3 3 4 3 5 0 6 1 7 2 6 1 5 0</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<geometry id="ID30">
|
||||
<mesh>
|
||||
<source id="ID31">
|
||||
<float_array id="ID35" count="24">18 18 0 -18 -18 0 -18 18 0 18 -18 0 18 -18 0 18 18 0 -18 -18 0 -18 18 0</float_array>
|
||||
<technique_common>
|
||||
<accessor count="8" source="#ID35" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID32">
|
||||
<float_array id="ID36" count="24">0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1</float_array>
|
||||
<technique_common>
|
||||
<accessor count="8" source="#ID36" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID34">
|
||||
<float_array id="ID37" count="8">-0.75 0.75 0.75 -0.75 0.75 0.75 -0.75 -0.75</float_array>
|
||||
<technique_common>
|
||||
<accessor count="4" source="#ID37" stride="2">
|
||||
<param name="S" type="float" />
|
||||
<param name="T" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID33">
|
||||
<input semantic="POSITION" source="#ID31" />
|
||||
<input semantic="NORMAL" source="#ID32" />
|
||||
</vertices>
|
||||
<triangles count="4" material="Material2">
|
||||
<input offset="0" semantic="VERTEX" source="#ID33" />
|
||||
<input offset="1" semantic="TEXCOORD" source="#ID34" />
|
||||
<p>0 0 1 1 2 2 1 1 0 0 3 3 4 3 5 0 6 1 7 2 6 1 5 0</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<geometry id="ID38">
|
||||
<mesh>
|
||||
<source id="ID41">
|
||||
<float_array id="ID44" count="240">0.8181818 0.8181818 2 0.8181818 -0.8181818 29 0.8181818 -0.8181818 2 0.8181818 0.8181818 29 0.8181818 0.8181818 29 0.8181818 0.8181818 2 0.8181818 -0.8181818 29 0.8181818 -0.8181818 2 11 -11 -5.53967e-018 0.8181818 0.8181818 2 0.8181818 -0.8181818 2 11 11 -5.671108e-018 11 11 -5.671108e-018 11 -11 -5.53967e-018 0.8181818 0.8181818 2 0.8181818 -0.8181818 2 -0.8181818 0.8181818 29 0.8181818 0.8181818 2 -0.8181818 0.8181818 2 0.8181818 0.8181818 29 0.8181818 0.8181818 29 -0.8181818 0.8181818 29 0.8181818 0.8181818 2 -0.8181818 0.8181818 2 0.8181818 -0.8181818 29 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 0.8181818 29 0.8181818 0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 -0.8181818 2 0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 -0.8181818 2 0.8181818 -0.8181818 2 11 11 -5.671108e-018 11 -11 -5.53967e-018 0 0 0 0 0 0 11 -11 -5.53967e-018 11 11 -5.671108e-018 -0.8181818 0.8181818 2 -11 11 5.53967e-018 -0.8181818 0.8181818 2 -11 11 5.53967e-018 -0.8181818 -0.8181818 2 -11 -11 5.671108e-018 -0.8181818 -0.8181818 2 -11 -11 5.671108e-018 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 -0.8181818 0.8181818 2 -0.8181818 0.8181818 2 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 11 -11 -5.53967e-018 -11 -11 5.671108e-018 0 0 0 0 0 0 -11 -11 5.671108e-018 11 -11 -5.53967e-018 11 11 -5.671108e-018 0 0 0 -11 11 5.53967e-018 -11 11 5.53967e-018 0 0 0 11 11 -5.671108e-018 0 0 0 -11 -11 5.671108e-018 -11 11 5.53967e-018 -11 11 5.53967e-018 -11 -11 5.671108e-018 0 0 0</float_array>
|
||||
<technique_common>
|
||||
<accessor count="80" source="#ID44" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID42">
|
||||
<float_array id="ID45" count="240">1 0 0 1 0 0 1 0 0 1 0 0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 0.9904913 0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 0.9904913 -0.0972804 -0.0972804 -0.9904913 -0.0972804 0.0972804 -0.9904913 -0.0972804 -0.0972804 -0.9904913 -0.0972804 0.0972804 -0.9904913 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -0.0972804 0.0972804 0.9904913 -0.0972804 0.0972804 0.9904913 0.0972804 -0.0972804 -0.9904913 0.0972804 -0.0972804 -0.9904913 -0.0972804 -0.0972804 0.9904913 -0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 -0.9904913 0.0972804 0.0972804 -0.9904913 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1</float_array>
|
||||
<technique_common>
|
||||
<accessor count="80" source="#ID45" stride="3">
|
||||
<param name="X" type="float" />
|
||||
<param name="Y" type="float" />
|
||||
<param name="Z" type="float" />
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID43">
|
||||
<input semantic="POSITION" source="#ID41" />
|
||||
<input semantic="NORMAL" source="#ID42" />
|
||||
</vertices>
|
||||
<triangles count="44" material="Material2">
|
||||
<input offset="0" semantic="VERTEX" source="#ID43" />
|
||||
<p>0 1 2 1 0 3 4 5 6 7 6 5 8 9 10 9 8 11 12 13 14 15 14 13 16 17 18 17 16 19 20 21 22 23 22 21 24 25 26 25 24 27 28 29 30 31 30 29 32 33 34 33 32 35 36 37 38 39 38 37 40 41 42 43 44 45 46 11 47 11 46 9 14 48 12 49 12 48 8 50 51 50 8 10 15 13 52 53 52 13 54 55 56 55 54 57 58 59 60 61 60 59 62 63 64 65 66 67 68 69 70 71 72 73 50 47 51 47 50 46 48 52 49 53 49 52 74 75 76 77 78 79</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_materials>
|
||||
<material id="ID5" name="Wood_Floor_Dark">
|
||||
<instance_effect url="#ID6" />
|
||||
</material>
|
||||
<material id="ID14" name="Maple">
|
||||
<instance_effect url="#ID15" />
|
||||
</material>
|
||||
<material id="ID39" name="Blue1">
|
||||
<instance_effect url="#ID40" />
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="ID6">
|
||||
<profile_COMMON>
|
||||
<newparam sid="ID8">
|
||||
<surface type="2D">
|
||||
<init_from>ID7</init_from>
|
||||
</surface>
|
||||
</newparam>
|
||||
<newparam sid="ID9">
|
||||
<sampler2D>
|
||||
<source>ID8</source>
|
||||
</sampler2D>
|
||||
</newparam>
|
||||
<technique sid="COMMON">
|
||||
<lambert>
|
||||
<diffuse>
|
||||
<texture texture="ID9" texcoord="UVSET0" />
|
||||
</diffuse>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
<effect id="ID15">
|
||||
<profile_COMMON>
|
||||
<newparam sid="ID17">
|
||||
<surface type="2D">
|
||||
<init_from>ID16</init_from>
|
||||
</surface>
|
||||
</newparam>
|
||||
<newparam sid="ID18">
|
||||
<sampler2D>
|
||||
<source>ID17</source>
|
||||
</sampler2D>
|
||||
</newparam>
|
||||
<technique sid="COMMON">
|
||||
<lambert>
|
||||
<diffuse>
|
||||
<texture texture="ID18" texcoord="UVSET0" />
|
||||
</diffuse>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
<effect id="ID40">
|
||||
<profile_COMMON>
|
||||
<technique sid="COMMON">
|
||||
<lambert>
|
||||
<diffuse>
|
||||
<color>0.3019608 0.4235294 0.5411765 1</color>
|
||||
</diffuse>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_images>
|
||||
<image id="ID7">
|
||||
<init_from>Wood_Floor_Dark.jpg</init_from>
|
||||
</image>
|
||||
<image id="ID16">
|
||||
<init_from>Maple_dark.jpg</init_from>
|
||||
</image>
|
||||
</library_images>
|
||||
<scene>
|
||||
<instance_visual_scene url="#ID1" />
|
||||
</scene>
|
||||
</COLLADA>
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>Cafe table</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model-1_4.sdf</sdf>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Nate Koenig</name>
|
||||
<email>nate@osrfoundation.org</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
A model of a cafe table.
|
||||
</description>
|
||||
|
||||
</model>
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="tray">
|
||||
|
||||
<xacro:property name="tray_width" value="0.2" />
|
||||
<xacro:property name="tray_length" value="0.32" />
|
||||
<xacro:property name="tray_height" value="0.05" />
|
||||
<xacro:property name="tray_thickness" value="0.015" />
|
||||
<xacro:property name="tray_density" value="100" />
|
||||
<xacro:property name="tray_base_name" value="tray_base" />
|
||||
|
||||
<xacro:macro name="tray_part" params="name width length height mass material xPos:=0 yPos:=0">
|
||||
<link name="${name}">
|
||||
<inertial>
|
||||
<mass value="${tray_density * width * length * height}" />
|
||||
|
||||
<inertia ixx="${(1/12) * tray_density * width * length * height * (length**2 + height**2)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(1/12) * tray_density * width * length * height * (width**2 + height**2)}" iyz="0.0"
|
||||
izz="${(1/12) * tray_density * width * length * height * (width**2 + length**2)}" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${width} ${length} ${height}" />
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${width} ${length} ${height}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<gazebo reference="${name}">
|
||||
<material>${material}</material>
|
||||
<mu1>1000</mu1>
|
||||
<mu2>1000</mu2>
|
||||
|
||||
<collision>
|
||||
<surface>
|
||||
<friction>
|
||||
<torsional>
|
||||
<coefficient>0.456</coefficient>
|
||||
<use_patch_radius>true</use_patch_radius>
|
||||
<patch_radius>0.05</patch_radius>
|
||||
</torsional>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</gazebo>
|
||||
|
||||
<xacro:if value="${xPos != 0 or yPos != 0}">
|
||||
<joint name="joint_${name}" type="fixed">
|
||||
<origin xyz="${xPos * (tray_width - tray_thickness) / 2} ${yPos * (tray_length - tray_thickness) / 2} ${(tray_height + tray_thickness) / 2}" />
|
||||
<parent link="${tray_base_name}" />
|
||||
<child link="${name}" />
|
||||
</joint>
|
||||
</xacro:if>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:tray_part name="${tray_base_name}" width="${tray_width}" length="${tray_length}" height="${tray_thickness}" mass="0.01" material="Gazebo/White" />
|
||||
<xacro:tray_part name="tray_wall0" width="${tray_width - 2 * tray_thickness}" length="${tray_thickness}" height="${tray_height}" mass="0.01" material="Gazebo/Red" yPos="-1" />
|
||||
<xacro:tray_part name="tray_wall1" width="${tray_width - 2 * tray_thickness}" length="${tray_thickness}" height="${tray_height}" mass="0.01" material="Gazebo/Red" yPos="1" />
|
||||
<xacro:tray_part name="tray_wall2" width="${tray_thickness}" length="${tray_length}" height="${tray_height}" mass="0.01" material="Gazebo/Blue" xPos="-1" />
|
||||
<xacro:tray_part name="tray_wall3" width="${tray_thickness}" length="${tray_length}" height="${tray_height}" mass="0.01" material="Gazebo/Blue" xPos="1" />
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>sorting_demo</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The sorting demo package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="pblasco@plainconcepts.com">Pablo Iñigo Blasco</maintainer>
|
||||
<maintainer email="mcsanchez@plainconcepts.com">Manuel Caballero Sánchez</maintainer>
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>Microsoft Copyright</license>
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/sorting_demo</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>intera_core_msgs</build_depend>
|
||||
<build_depend>gazebo_msgs</build_depend>
|
||||
<build_depend>joint_state_controller</build_depend>
|
||||
<build_depend>position_controllers</build_depend>
|
||||
<build_depend>velocity_controllers</build_depend>
|
||||
<build_depend>force_torque_sensor_controller</build_depend>
|
||||
<build_depend>imu_sensor_controller</build_depend>
|
||||
<build_depend>ros_control</build_depend>
|
||||
<build_depend>joint_trajectory_controller</build_depend>
|
||||
<build_depend>gripper_action_controller</build_depend>
|
||||
<build_depend>gazebo_ros_control</build_depend>
|
||||
<build_depend>gazebo_plugins</build_depend>
|
||||
<build_depend>rospy-message-converter</build_depend>
|
||||
<build_depend>gazebo_plugins</build_depend>
|
||||
<build_depend>python-flask</build_depend>
|
||||
<build_depend>python-catkin-tools</build_depend>
|
||||
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>rospack</exec_depend>
|
||||
<exec_depend>intera_core_msgs</exec_depend>
|
||||
<exec_depend>intera_interface</exec_depend>
|
||||
<exec_depend>sawyer_gazebo</exec_depend>
|
||||
<exec_depend>gazebo_ros</exec_depend>
|
||||
<exec_depend>gazebo_msgs</exec_depend>
|
||||
<exec_depend>joint_state_controller</exec_depend>
|
||||
|
||||
<exec_depend>position_controllers</exec_depend>
|
||||
<exec_depend>velocity_controllers</exec_depend>
|
||||
<exec_depend>force_torque_sensor_controller</exec_depend>
|
||||
<exec_depend>imu_sensor_controller</exec_depend>
|
||||
<exec_depend>ros_control</exec_depend>
|
||||
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||
<exec_depend>gripper_action_controller</exec_depend>
|
||||
<exec_depend>rospy-message-converter</exec_depend>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
<gazebo_ros gazebo_model_path="${prefix}/.." />
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#!/usr/bin/env python
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
"""
|
||||
setup_args = generate_distutils_setup(
|
||||
package=['sorting_demo'],
|
||||
package_dir={'': 'src'},
|
||||
)
|
||||
|
||||
setup(**setup_args)
|
||||
"""
|
||||
|
||||
d = generate_distutils_setup()
|
||||
d['packages'] = ['sorting_demo',
|
||||
'sorting_demo.utils',
|
||||
'sorting_demo.object_detection',
|
||||
'intera_interface', 'intera_control',
|
||||
'intera_dataflow',
|
||||
'intera_io',
|
||||
'intera_joint_trajectory_action',
|
||||
'intera_motion_interface']
|
||||
d['package_dir'] = {'': 'src', 'utils': 'src/utils'}
|
||||
|
||||
setup(**d)
|
||||
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 153 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 153 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 151 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 151 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 155 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 13 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 153 KiB |
Двоичный файл не отображается.
|
После Ширина: | Высота: | Размер: 152 KiB |
|
|
@ -0,0 +1,42 @@
|
|||
import re
|
||||
from rospy_message_converter import message_converter
|
||||
|
||||
|
||||
class BlockState:
|
||||
regex = re.compile(r'block(\d+)\.*')
|
||||
|
||||
def __init__(self, id, pose):
|
||||
self.id = id
|
||||
self.pose = pose
|
||||
self.color = None
|
||||
self.homogeneous_transform = None
|
||||
search = BlockState.regex.search(id)
|
||||
self.num = int(search.group(1))
|
||||
self.final_pose = None
|
||||
|
||||
# the 2d blob point estimabion based on the head_image processing
|
||||
self.hue_estimation = None
|
||||
|
||||
# the color estimation based on the head image processing
|
||||
self.hue_estimation = None
|
||||
|
||||
# the 3d pose estimation from the head image processing
|
||||
self.headview_pose_estimation = None
|
||||
|
||||
def __str__(self):
|
||||
return "[Block estpos = %s]" % str(self.headview_pose_estimation)
|
||||
|
||||
def get_state(self):
|
||||
return {"id": self.id, "table_pose": message_converter.convert_ros_message_to_dictionary(self.final_pose),
|
||||
"color": self.color}
|
||||
|
||||
@staticmethod
|
||||
def is_block(id):
|
||||
num = BlockState.regex.search(id)
|
||||
return num is not None
|
||||
|
||||
def get_color(self):
|
||||
return self.color
|
||||
|
||||
def is_color(self, color):
|
||||
return color.upper() in self.color.upper()
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
class Table:
|
||||
def __init__(self):
|
||||
self.blocks = []
|
||||
|
||||
def get_blocks(self):
|
||||
return self.blocks
|
||||
|
||||
def get_state(self):
|
||||
return {"blocks": [b.get_state() for b in self.blocks]}
|
||||
|
||||
def notify_block_removed(self, b):
|
||||
self.blocks.remove(b)
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
import copy
|
||||
import re
|
||||
import rospy
|
||||
from rospy_message_converter import message_converter
|
||||
|
||||
|
||||
class TrayState:
|
||||
regex = re.compile(r'tray(\d+)\.*')
|
||||
|
||||
def __init__(self, id, pose):
|
||||
self.id = id
|
||||
self.pose = pose
|
||||
self.final_pose = None
|
||||
|
||||
search = TrayState.regex.search(id)
|
||||
self.num = int(search.group(1))
|
||||
self.blocks = []
|
||||
|
||||
@staticmethod
|
||||
def is_tray(id):
|
||||
num = TrayState.regex.search(id)
|
||||
return num is not None
|
||||
|
||||
def notify_contains_block(self, block):
|
||||
self.blocks.append(block)
|
||||
|
||||
def get_tray_pick_location(self):
|
||||
"""
|
||||
provides the grasping pose for the tray
|
||||
|
||||
:return: geometry_msg/Pose
|
||||
"""
|
||||
copyfinalpose = copy.deepcopy(self.final_pose)
|
||||
copyfinalpose.position.y -= 0.15
|
||||
copyfinalpose.position.z -= 0.02
|
||||
return copyfinalpose
|
||||
|
||||
def get_tray_place_block_location(self):
|
||||
#return self.final_pose
|
||||
copyfinalpose = copy.deepcopy(self.final_pose)
|
||||
|
||||
yoffset = -0.08 + 0.075 * len(self.blocks)
|
||||
copyfinalpose.position.y -= yoffset
|
||||
copyfinalpose.position.z += 0.03
|
||||
|
||||
rospy.logwarn("Tray Place location (objects %d, %lf): " % (len(self.blocks), yoffset) + str(copyfinalpose))
|
||||
|
||||
return copyfinalpose
|
||||
|
||||
def reset(self):
|
||||
self.blocks = []
|
||||
|
||||
def get_state(self):
|
||||
return {"id": self.id, "pose": message_converter.convert_ros_message_to_dictionary(self.final_pose), "blocks": [b.get_state() for b in self.blocks]}
|
||||
|
||||
def __str__(self):
|
||||
return "Tray: "+ str(self.id) +",num: "+str(self.num)+" -> "+ str(len(self.blocks)) + " items"
|
||||
|
|
@ -0,0 +1,324 @@
|
|||
#!/usr/bin/python
|
||||
import sys
|
||||
import Queue
|
||||
import numpy
|
||||
import os
|
||||
|
||||
from matplotlib import pyplot
|
||||
|
||||
import rospy
|
||||
import tf
|
||||
import cv2
|
||||
import rospkg
|
||||
|
||||
from sensor_msgs.msg import CameraInfo
|
||||
from image_geometry import PinholeCameraModel
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
|
||||
from visualization_msgs.msg import *
|
||||
|
||||
import intera_interface
|
||||
|
||||
class CameraHelper:
|
||||
"""
|
||||
A helper class to take pictures with the Sawyer camera and unproject points from the images
|
||||
"""
|
||||
def __init__(self, camera_name, base_frame, table_height):
|
||||
"""
|
||||
Initialize the instance
|
||||
|
||||
:param camera_name: The camera name. One of (head_camera, right_hand_camera)
|
||||
:param base_frame: The frame for the robot base
|
||||
:param table_height: The table height with respect to base_frame
|
||||
"""
|
||||
self.camera_name = camera_name
|
||||
self.base_frame = base_frame
|
||||
self.table_height = table_height
|
||||
|
||||
self.image_queue = Queue.Queue()
|
||||
self.pinhole_camera_model = PinholeCameraModel()
|
||||
self.tf_listener = tf.TransformListener()
|
||||
|
||||
camera_info_topic = "/io/internal_camera/{}/camera_info".format(camera_name)
|
||||
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
|
||||
|
||||
self.pinhole_camera_model.fromCameraInfo(camera_info)
|
||||
|
||||
cameras = intera_interface.Cameras()
|
||||
cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True)
|
||||
|
||||
def __show_image_callback(self, img_data):
|
||||
"""
|
||||
Image callback for the camera
|
||||
|
||||
:param img_data: The image received from the camera
|
||||
"""
|
||||
if self.image_queue.empty():
|
||||
self.image_queue.put(img_data)
|
||||
|
||||
def take_single_picture(self):
|
||||
"""
|
||||
Take one picture from the specified camera
|
||||
|
||||
:returns: The image
|
||||
"""
|
||||
with self.image_queue.mutex:
|
||||
self.image_queue.queue.clear()
|
||||
|
||||
cameras = intera_interface.Cameras()
|
||||
|
||||
cameras.start_streaming(self.camera_name)
|
||||
|
||||
image_data = self.image_queue.get(block=True, timeout=None)
|
||||
|
||||
cameras.stop_streaming(self.camera_name)
|
||||
|
||||
return image_data
|
||||
|
||||
def project_point_on_table(self, point):
|
||||
"""
|
||||
Projects the 2D point from the camera image on the table
|
||||
|
||||
:param point: The 2D point in the form (x, y)
|
||||
:return: The 3D point in the coordinate space of the frame that was specified when the object was initialized
|
||||
"""
|
||||
|
||||
# Get camera frame name
|
||||
camera_frame = self.pinhole_camera_model.tfFrame()
|
||||
|
||||
# Check that both frames exist
|
||||
if self.tf_listener.frameExists(self.base_frame) and self.tf_listener.frameExists(camera_frame):
|
||||
# Get transformation
|
||||
time = self.tf_listener.getLatestCommonTime(self.base_frame, camera_frame)
|
||||
camera_translation_base, camera_orientation_base = self.tf_listener.lookupTransform(self.base_frame, camera_frame, time)
|
||||
|
||||
# Unproject point
|
||||
unprojected_ray_camera = self.pinhole_camera_model.projectPixelTo3dRay(point)
|
||||
|
||||
# Rotate ray based on the frame transformation
|
||||
camera_frame_rotation_matrix = tf.transformations.quaternion_matrix(camera_orientation_base)
|
||||
unprojected_ray_base = numpy.dot(camera_frame_rotation_matrix[:3,:3], unprojected_ray_camera)
|
||||
|
||||
# Intersect ray with base plane
|
||||
point_height = camera_translation_base[2] - self.table_height
|
||||
factor = -point_height / unprojected_ray_base[2]
|
||||
intersection = camera_translation_base + unprojected_ray_base * factor
|
||||
|
||||
return intersection
|
||||
|
||||
def get_blobs_info(cv_image):
|
||||
"""
|
||||
Gets information about the colored blobs in the image
|
||||
|
||||
:param cv_image: The OpenCV image to get blobs from
|
||||
:return: A dictionary containing an array of points for each colored blob found, represented by its hue.
|
||||
For example, in an image with 2 red blobs and 3 blue blobs, it will return
|
||||
{0: [(639, 558), (702, 555)], 120: [(567, 550), (698, 515), (648, 515)]}
|
||||
"""
|
||||
|
||||
# Show original image
|
||||
#cv2.imshow("Original image", cv_image)
|
||||
|
||||
# Apply blur
|
||||
BLUR_SIZE = 3
|
||||
cv_image_blur = cv2.GaussianBlur(cv_image, (BLUR_SIZE, BLUR_SIZE), 0)
|
||||
#cv2.imshow("Blur", cv_image_blur)
|
||||
|
||||
# Apply ROI mask
|
||||
mask_path = rospkg.RosPack().get_path('sorting_demo') + "/share/head-mask.png"
|
||||
cv_mask = cv2.imread(mask_path)
|
||||
cv_mask = cv2.cvtColor(cv_mask, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
cv_image_masked = cv2.bitwise_and(cv_image_blur, cv_image_blur, mask = cv_mask)
|
||||
#cv2.imshow("Masked original", cv_image_masked)
|
||||
|
||||
# HSV split
|
||||
cv_image_hsv = cv2.cvtColor(cv_image_masked, cv2.COLOR_BGR2HSV)
|
||||
cv_image_h, cv_image_s, cv_image_v = cv2.split(cv_image_hsv)
|
||||
#cv2.imshow("Image H", cv_image_h)
|
||||
#cv2.imshow("Image S", cv_image_s)
|
||||
#cv2.imshow("Image V", cv_image_v)
|
||||
|
||||
# Apply CLAHE to Value channel
|
||||
CLAHE_SIZE = 16
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(CLAHE_SIZE, CLAHE_SIZE))
|
||||
cv_image_v_clahe = clahe.apply(cv_image_v)
|
||||
|
||||
# Merge channels
|
||||
cv_image_clahe = cv2.merge((cv_image_h, cv_image_s, cv_image_v_clahe))
|
||||
#cv2.imshow("CLAHE", cv_image_clahe)
|
||||
|
||||
# Multiply Saturation and Value channels to separate the cubes, removing the table
|
||||
cv_image_sv_multiplied = cv2.multiply(cv_image_s, cv_image_v_clahe, scale=1/255.0)
|
||||
#cv2.imshow("Image S*V", cv_image_sv_multiplied)
|
||||
|
||||
# Binarize the result
|
||||
BIN_THRESHOLD = 127
|
||||
#ret, cv_image_binary = cv2.threshold(cv_image_sv_multiplied, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
|
||||
ret, cv_image_binary = cv2.threshold(cv_image_sv_multiplied, BIN_THRESHOLD, 255, cv2.THRESH_BINARY)
|
||||
#cv2.imshow("Threshold", cv_image_binary)
|
||||
|
||||
# Calculate H-channel histogram, applying the binarized image as mask
|
||||
cv_image_h_histogram = cv2.calcHist([cv_image_h], [0], cv_image_binary, [256], [0, 255])
|
||||
|
||||
# Smoothen the histogram to find local maxima
|
||||
HISTOGRAM_BLUR_SIZE = 11
|
||||
histogram_length = len(cv_image_h_histogram)
|
||||
cv_image_h_histogram_wrap = cv2.repeat(cv_image_h_histogram, 3, 1)
|
||||
cv_image_h_histogram_smooth = cv2.GaussianBlur(cv_image_h_histogram_wrap, (HISTOGRAM_BLUR_SIZE, HISTOGRAM_BLUR_SIZE), 0)
|
||||
cv_image_h_histogram_cut = cv_image_h_histogram_smooth[histogram_length : 2 * histogram_length]
|
||||
|
||||
# Collect high peaks
|
||||
PEAK_RATIO = 0.2
|
||||
histogram_peak_cut = PEAK_RATIO * max(cv_image_h_histogram_cut)
|
||||
histogram_peaks = [i for i in range(len(cv_image_h_histogram_cut))
|
||||
if cv_image_h_histogram_cut[(i - 1) % histogram_length] < cv_image_h_histogram_cut[i] > cv_image_h_histogram_cut[(i + 1) % histogram_length]]
|
||||
histogram_high_peaks = filter(lambda x : cv_image_h_histogram_cut[x] > histogram_peak_cut, histogram_peaks)
|
||||
#print(histogram_high_peaks)
|
||||
#pyplot.plot(cv_image_h_histogram_cut)
|
||||
#pyplot.show()
|
||||
|
||||
# Process every color found in the histogram
|
||||
blob_info = {}
|
||||
cv_image_contours_debug = cv2.cvtColor(cv_image_binary, cv2.COLOR_GRAY2BGR)
|
||||
for current_hue in histogram_high_peaks:
|
||||
# Perform a Hue rotation that will be used to make detecting the edge colors easier (red in HSV corresponds to both 0 and 180)
|
||||
HUE_AMPLITUDE = 5
|
||||
cv_image_h_rotated = cv_image_h.copy()
|
||||
cv_image_h_rotated[:] -= current_hue
|
||||
cv_image_h_rotated[:] += HUE_AMPLITUDE
|
||||
|
||||
# Binarize using range function
|
||||
cv_image_h_inrange = cv2.inRange(cv_image_h_rotated, 0, HUE_AMPLITUDE * 2)
|
||||
|
||||
# Apply binary mask (consider that both black and the edge color have hue 0)
|
||||
cv_image_h_masked = cv2.bitwise_and(cv_image_h_inrange, cv_image_h_inrange, mask=cv_image_binary)
|
||||
|
||||
# Erode
|
||||
EROSION_SIZE = 5
|
||||
erosion_kernel = numpy.ones((EROSION_SIZE, EROSION_SIZE), numpy.uint8)
|
||||
cv_image_h_eroded = cv2.erode(cv_image_h_masked, erosion_kernel)
|
||||
#cv2.imshow("inRange {}".format(histogram_high_peaks.index(current_hue)), cv_image_h_eroded)
|
||||
|
||||
# Find convex contours
|
||||
_, contours, _ = cv2.findContours(cv_image_h_eroded.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
|
||||
convex_contours = [cv2.convexHull(cnt) for cnt in contours]
|
||||
|
||||
contour_color_hsv = numpy.array([[[current_hue, 255, 255]]], numpy.uint8)
|
||||
contour_color_rgb = cv2.cvtColor(contour_color_hsv, cv2.COLOR_HSV2BGR)[0][0].tolist()
|
||||
#cv2.drawContours(cv_image_contours_debug, convex_contours, -1, contour_color_rgb, 1)
|
||||
|
||||
# Find centroids
|
||||
contour_moments = [cv2.moments(cnt) for cnt in convex_contours]
|
||||
contour_centroids = [(int(moments["m10"] / moments["m00"]), int(moments["m01"] / moments["m00"])) for moments in contour_moments if moments["m00"] != 0]
|
||||
for (cx, cy) in contour_centroids:
|
||||
cv2.circle(cv_image_contours_debug, (cx, cy), 3, contour_color_rgb, -1)
|
||||
|
||||
# Collect data
|
||||
blob_info[current_hue] = contour_centroids
|
||||
|
||||
#cv2.imshow("Convex contours", cv_image_contours_debug)
|
||||
|
||||
return blob_info
|
||||
|
||||
def __publish_marker(publisher, index, point):
|
||||
"""
|
||||
Publishes a cube-shaped marker
|
||||
|
||||
:param index: The marker index
|
||||
:param point: The 3D position of the marker
|
||||
"""
|
||||
marker = Marker()
|
||||
marker.header.frame_id = "base"
|
||||
marker.id = index
|
||||
marker.type = Marker.CUBE
|
||||
marker.scale.x = 0.04
|
||||
marker.scale.y = 0.04
|
||||
marker.scale.z = 0.04
|
||||
marker.color.r = 1.0
|
||||
marker.color.g = 1.0
|
||||
marker.color.b = 0.0
|
||||
marker.color.a = 1.0
|
||||
marker.pose.position.x = point[0]
|
||||
marker.pose.position.y = point[1]
|
||||
marker.pose.position.z = point[2]
|
||||
|
||||
publisher.publish(marker)
|
||||
|
||||
def test_ros():
|
||||
"""
|
||||
Test the blob detection and CameraHelper class using ROS
|
||||
"""
|
||||
rospy.init_node('cv_detection')
|
||||
|
||||
camera_name = "head_camera"
|
||||
|
||||
TABLE_HEIGHT = -0.12
|
||||
camera_helper = CameraHelper(camera_name, "base", TABLE_HEIGHT)
|
||||
|
||||
bridge = CvBridge()
|
||||
|
||||
publisher = rospy.Publisher("cube_position_estimation", Marker, queue_size=10)
|
||||
|
||||
try:
|
||||
while not rospy.is_shutdown():
|
||||
# Take picture
|
||||
img_data = camera_helper.take_single_picture()
|
||||
|
||||
# Convert to OpenCV format
|
||||
cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
|
||||
|
||||
# Save for debugging
|
||||
#cv2.imwrite("debug.png", cv_image)
|
||||
|
||||
# Get color blobs info
|
||||
blob_info = get_blobs_info(cv_image)
|
||||
|
||||
# Project the points on 3D space
|
||||
points = [y for x in blob_info.values() for y in x]
|
||||
|
||||
for index, point in enumerate(points):
|
||||
projected = camera_helper.project_point_on_table(point)
|
||||
__publish_marker(publisher, index, projected)
|
||||
|
||||
# Wait for a key press
|
||||
cv2.waitKey(1)
|
||||
|
||||
rospy.sleep(0.1)
|
||||
except CvBridgeError, err:
|
||||
rospy.logerr(err)
|
||||
|
||||
# Exit
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
def test_debug():
|
||||
"""
|
||||
Test the blob detection using images on disk
|
||||
"""
|
||||
|
||||
# Get files
|
||||
path = rospkg.RosPack().get_path('sorting_demo') + "/share"
|
||||
files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f)) and "head-mask" not in f]
|
||||
#print(files)
|
||||
|
||||
# Process files
|
||||
for f in files:
|
||||
# Get image path
|
||||
image_path = os.path.join(path, f)
|
||||
print(image_path)
|
||||
|
||||
# Read image
|
||||
cv_image = cv2.imread(image_path)
|
||||
|
||||
# Get color blobs info
|
||||
blob_info = get_blobs_info(cv_image)
|
||||
print(blob_info)
|
||||
|
||||
# Wait for a key press
|
||||
cv2.waitKey(0)
|
||||
|
||||
# Exit
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(test_debug())
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#!/usr/bin/python
|
||||
CUBE_EDGE_LENGTH = 0.04
|
||||
|
||||
BLOCK_COLOR_MAPPINGS = [
|
||||
{"material": "Gazebo/Green"},
|
||||
{"material": "Gazebo/Red"},
|
||||
{"material": "Gazebo/Blue"},
|
||||
{"material": "Gazebo/Green"},
|
||||
{"material": "Gazebo/Red"},
|
||||
{"material": "Gazebo/Blue"},
|
||||
{"material": "Gazebo/Red"},
|
||||
{"material": "Gazebo/Blue"},
|
||||
{"material": "Gazebo/Green"}
|
||||
]
|
||||
|
||||
TRAY_COLORS = ["Red", "Green", "Blue"]
|
||||
|
|
@ -0,0 +1,331 @@
|
|||
#!/usr/bin/python
|
||||
import copy
|
||||
import re
|
||||
|
||||
import math
|
||||
import rospy
|
||||
import tf.transformations
|
||||
from cv_bridge import CvBridge
|
||||
from gazebo_msgs.msg import LinkStates, sys
|
||||
|
||||
from concepts.block import BlockState
|
||||
from concepts.tray import TrayState
|
||||
from concepts.table import Table
|
||||
|
||||
from cv_detection import CameraHelper, get_blobs_info
|
||||
|
||||
from utils.mathutils import *
|
||||
import demo_constants
|
||||
from threading import RLock
|
||||
|
||||
|
||||
class EnvironmentEstimation:
|
||||
def __init__(self):
|
||||
"""
|
||||
|
||||
"""
|
||||
self.gazebo_trays = []
|
||||
self.gazebo_blocks = []
|
||||
|
||||
self.trays = []
|
||||
self.blocks = []
|
||||
|
||||
self.tf_broacaster = tf.TransformBroadcaster()
|
||||
|
||||
# initial simulated implementation
|
||||
pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self._links_callback, queue_size=10)
|
||||
|
||||
self.gazebo_world_to_ros_transform = None
|
||||
self.original_blocks_poses_ = None
|
||||
self.mutex = RLock()
|
||||
|
||||
camera_name = "head_camera"
|
||||
TABLE_HEIGHT = -0.12
|
||||
self.camera_helper = CameraHelper(camera_name, "base", TABLE_HEIGHT)
|
||||
self.bridge = CvBridge()
|
||||
|
||||
self.block_pose_estimation_head_camera = None
|
||||
|
||||
self.table = Table()
|
||||
|
||||
def identify_block_from_aproximated_point(self, projected):
|
||||
bestindex = -1
|
||||
bestdist = sys.float_info.max
|
||||
for index, b in enumerate(self.blocks):
|
||||
p1 = b.pose.position
|
||||
p2 = projected
|
||||
dx = p1.x - p2[0]
|
||||
dy = p1.y - p2[1]
|
||||
dz = p1.z - p2[2]
|
||||
|
||||
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
|
||||
|
||||
if dist < bestdist:
|
||||
bestdist = dist
|
||||
bestindex = index
|
||||
|
||||
if bestindex != -1:
|
||||
return self.blocks[bestindex]
|
||||
else:
|
||||
return None
|
||||
|
||||
def compute_block_pose_estimations_from_head_camera(self):
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
img_data = self.camera_helper.take_single_picture()
|
||||
|
||||
# Convert to OpenCV format
|
||||
cv_image = self.bridge.imgmsg_to_cv2(img_data, "bgr8")
|
||||
|
||||
blobs_info = get_blobs_info(cv_image)
|
||||
|
||||
index = 0
|
||||
ptinfos = []
|
||||
for huekey in blobs_info.keys():
|
||||
points = blobs_info[huekey]
|
||||
rospy.logwarn("blob position[%d]: %s" % (index, str(points)))
|
||||
for point in points:
|
||||
ptinfos.append([huekey, point])
|
||||
index += 1
|
||||
|
||||
self.table.blocks = []
|
||||
|
||||
for huekey, point2d in ptinfos:
|
||||
projected = self.camera_helper.project_point_on_table(point2d)
|
||||
rospy.logwarn("projected: %s" % str(projected))
|
||||
|
||||
block = self.identify_block_from_aproximated_point(projected)
|
||||
if block is None:
|
||||
continue
|
||||
|
||||
self.table.blocks.append(block)
|
||||
|
||||
block.headview_proj_estimation = point2d
|
||||
|
||||
block.headview_proj_estimation = projected
|
||||
block.hue_estimation = huekey
|
||||
block.headview_pose_estimation = Pose(
|
||||
position=Point(x=projected[0], y=projected[1], z=projected[2]),
|
||||
orientation=Quaternion(x=0, y=0, z=0, w=1))
|
||||
|
||||
rospy.logwarn("blob identified: " + str(block))
|
||||
|
||||
for b in self.blocks:
|
||||
rospy.logwarn(b)
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def _links_callback(self, links):
|
||||
"""
|
||||
string[] name
|
||||
geometry_msgs/Pose[] pose
|
||||
geometry_msgs/Point position
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
geometry_msgs/Twist[] twist
|
||||
geometry_msgs/Vector3 linear
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
geometry_msgs/Vector3 angular
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
:param links:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
blocks = []
|
||||
trays = []
|
||||
|
||||
base_index = [i for i, name in enumerate(links.name) if name == "sawyer::base"][0]
|
||||
self.gazebo_world_to_ros_transform = links.pose[base_index]
|
||||
|
||||
for i, name in enumerate(links.name):
|
||||
pose = links.pose[i]
|
||||
|
||||
if BlockState.is_block(name):
|
||||
item = self.get_block(name)
|
||||
if item is None:
|
||||
# rospy.logwarn("block create name: "+ name)
|
||||
item = BlockState(id=name, pose=pose)
|
||||
item.color = demo_constants.BLOCK_COLOR_MAPPINGS[item.num]["material"].replace("Gazebo/","")
|
||||
else:
|
||||
item.pose = pose
|
||||
|
||||
blocks.append(item)
|
||||
elif TrayState.is_tray(name):
|
||||
item = self.get_tray(name)
|
||||
# item = None
|
||||
if item is None:
|
||||
item = TrayState(id=name, pose=pose)
|
||||
item.color = demo_constants.TRAY_COLORS[item.num].replace("Gazebo/","")
|
||||
else:
|
||||
item.pose = pose
|
||||
|
||||
trays.append(item)
|
||||
else:
|
||||
continue
|
||||
|
||||
# rospy.logwarn("simulated object state: " + name + " -> " + item.num)
|
||||
|
||||
self.gazebo_blocks = blocks
|
||||
self.gazebo_trays = trays
|
||||
except Exception as ex:
|
||||
rospy.logerr(ex.message)
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
this method basically double buffers the state of block and trays. It also publishes tf.
|
||||
For the simulated case it copies from gazebo_blocks and gazebo_trays to blocks and trays
|
||||
:return:
|
||||
"""
|
||||
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
|
||||
collections = [self.gazebo_blocks, self.gazebo_trays]
|
||||
|
||||
blocks = []
|
||||
trays = []
|
||||
|
||||
# publish tfs
|
||||
basehomopose = get_homo_matrix_from_pose_msg(self.gazebo_world_to_ros_transform, tag="base")
|
||||
|
||||
for items in collections:
|
||||
for item in items:
|
||||
|
||||
# block homogeneous transform
|
||||
homo_pose = get_homo_matrix_from_pose_msg(item.pose, tag="block")
|
||||
|
||||
# rospy.logwarn("TF PUBLISH..." + str(homo_pose))
|
||||
# rospy.logwarn("item state: " + str(item))
|
||||
|
||||
transf_homopose = inverse_compose(basehomopose, homo_pose)
|
||||
|
||||
trans = tf.transformations.translation_from_matrix(transf_homopose)
|
||||
quat = tf.transformations.quaternion_from_matrix(transf_homopose)
|
||||
|
||||
self.tf_broacaster.sendTransform(trans,
|
||||
quat,
|
||||
rospy.Time.now(),
|
||||
item.id,
|
||||
"world")
|
||||
|
||||
item.final_pose = homotransform_to_pose_msg(transf_homopose)
|
||||
|
||||
if isinstance(item, BlockState):
|
||||
blocks.append(item)
|
||||
elif isinstance(item, TrayState):
|
||||
trays.append(item)
|
||||
# else:
|
||||
# rospy.logwarn("DETECTED ITEM:" + str(item))
|
||||
|
||||
self.blocks = blocks
|
||||
self.trays = trays
|
||||
|
||||
# rospy.logwarn("GAZEBO blocks update lenght: %d"%len(self.gazebo_blocks))
|
||||
# rospy.logwarn("blocks update lenght: %d"%len(self.blocks))
|
||||
if self.original_blocks_poses_ is None:
|
||||
self.original_blocks_poses_ = [copy.deepcopy(block.final_pose) for block in blocks]
|
||||
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_blocks(self):
|
||||
"""
|
||||
:return array of (name, geometry_msgs.msg.Pose)
|
||||
"""
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
return [b for b in self.blocks]
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_block(self, id):
|
||||
"""
|
||||
|
||||
:param id:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
|
||||
filtered_blocks = [block for block in self.blocks if block.id == id]
|
||||
if len(filtered_blocks) == 0:
|
||||
return None
|
||||
else:
|
||||
return filtered_blocks[0]
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_original_block_poses(self):
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
return [copy.deepcopy(p) for p in self.original_blocks_poses_]
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_tray(self, id):
|
||||
"""
|
||||
|
||||
:param id:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
filtered_trays = [tray for tray in self.trays if tray.id == id]
|
||||
if len(filtered_trays) == 0:
|
||||
return None
|
||||
else:
|
||||
return filtered_trays[0]
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_tray_by_color(self, color):
|
||||
"""
|
||||
|
||||
:param id:
|
||||
:return:
|
||||
"""
|
||||
|
||||
color = color.replace("Gazebo/", "")
|
||||
rospy.logwarn("by color: " + str(color))
|
||||
rospy.logwarn("by color: " + str(self.trays))
|
||||
|
||||
filtered_trays = [tray for tray in self.trays if tray.color == color]
|
||||
if len(filtered_trays) == 0:
|
||||
return None
|
||||
else:
|
||||
return filtered_trays[0]
|
||||
|
||||
def get_tray_by_num(self, num):
|
||||
"""
|
||||
|
||||
:param num:
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("by num: " + str(num))
|
||||
rospy.logwarn("by trays: " + str([t.num for t in self.trays]))
|
||||
|
||||
filtered_trays = [tray for tray in self.trays if int(tray.num) == int(num)]
|
||||
if len(filtered_trays) == 0:
|
||||
return None
|
||||
else:
|
||||
return filtered_trays[0]
|
||||
|
||||
def get_trays(self):
|
||||
"""
|
||||
:return array of (name, geometry_msgs.msg.Pose)
|
||||
"""
|
||||
return self.trays
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
#!/usr/bin/python
|
||||
import random
|
||||
import sys
|
||||
|
||||
import rospkg
|
||||
import rospy
|
||||
import xacro
|
||||
|
||||
from geometry_msgs.msg import Pose, Point, Quaternion
|
||||
from gazebo_msgs.srv import SpawnModel, DeleteModel
|
||||
|
||||
from demo_constants import BLOCK_COLOR_MAPPINGS, CUBE_EDGE_LENGTH
|
||||
|
||||
|
||||
def spawn_urdf(name, description_xml, pose, reference_frame):
|
||||
rospy.wait_for_service('/gazebo/spawn_urdf_model')
|
||||
try:
|
||||
spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
|
||||
resp_urdf = spawn_urdf(name, description_xml, "/", pose, reference_frame)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
|
||||
def load_xacro_file(file_path, mappings):
|
||||
urdf_doc = xacro.process_file(file_path, mappings=mappings)
|
||||
urdf_xml = urdf_doc.toprettyxml(indent=' ', encoding='utf-8')
|
||||
urdf_xml = urdf_xml.replace('\n', '')
|
||||
return urdf_xml
|
||||
|
||||
|
||||
def spawn_xacro_model(name, path, pose, reference_frame, mappings):
|
||||
description_xml = load_xacro_file(path, mappings)
|
||||
spawn_urdf(name, description_xml, pose, reference_frame)
|
||||
|
||||
|
||||
def spawn_urdf_model(name, path, pose, reference_frame):
|
||||
description_xml = ''
|
||||
with open(path, "r") as model_file:
|
||||
description_xml = model_file.read().replace('\n', '')
|
||||
|
||||
spawn_urdf(name, description_xml, pose, reference_frame)
|
||||
|
||||
|
||||
def spawn_sdf_model(name, path, pose, reference_frame):
|
||||
# Load Model SDF
|
||||
description_xml = ''
|
||||
with open(path, "r") as model_file:
|
||||
description_xml = model_file.read().replace('\n', '')
|
||||
|
||||
# Spawn Model SDF
|
||||
rospy.wait_for_service('/gazebo/spawn_sdf_model')
|
||||
try:
|
||||
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
|
||||
resp_sdf = spawn_sdf(name, description_xml, "/", pose, reference_frame)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
|
||||
|
||||
|
||||
def load_gazebo_models():
|
||||
model_list = []
|
||||
|
||||
world_reference_frame = "world"
|
||||
|
||||
# sorting_demo model path
|
||||
sorting_demo_models_path = rospkg.RosPack().get_path('sorting_demo') + "/models/"
|
||||
|
||||
# Spawn Blocks Table
|
||||
blocks_table_name = "blocks_table"
|
||||
blocks_table_path = sorting_demo_models_path + "table/model.sdf"
|
||||
blocks_table_pose = Pose(position=Point(x=0.75, y=0.0, z=0.0))
|
||||
|
||||
spawn_sdf_model(blocks_table_name, blocks_table_path, blocks_table_pose, world_reference_frame)
|
||||
model_list.append(blocks_table_name)
|
||||
|
||||
# Spawn Trays Table
|
||||
trays_table_name = "trays_table"
|
||||
trays_table_path = sorting_demo_models_path + "table/model.sdf"
|
||||
trays_table_pose = Pose(position=Point(x=0.0, y=0.95, z=0.0))
|
||||
|
||||
spawn_sdf_model(trays_table_name, trays_table_path, trays_table_pose, world_reference_frame)
|
||||
model_list.append(trays_table_name)
|
||||
|
||||
# Spawn trays
|
||||
tray_path = sorting_demo_models_path + "tray/tray.urdf.xacro"
|
||||
|
||||
tray_poses = [Pose(position=Point(x=-0.2, y=0.7, z=0.9)),
|
||||
Pose(position=Point(x=0.05, y=0.7, z=0.9)),
|
||||
Pose(position=Point(x=0.3, y=0.7, z=0.9))]
|
||||
|
||||
for (i, pose) in enumerate(tray_poses):
|
||||
name = "tray{}".format(i)
|
||||
spawn_xacro_model(name, tray_path, pose, world_reference_frame, {})
|
||||
model_list.append(name)
|
||||
|
||||
# Spawn blocks
|
||||
block_path = sorting_demo_models_path + "block/block.urdf.xacro"
|
||||
|
||||
block_poses = []
|
||||
k = 3
|
||||
for i in xrange(k):
|
||||
for j in xrange(k):
|
||||
block_poses.append(Pose(position=Point(x= 0.45 + j*0.12 +random.uniform(-1, 1)*0.03 , y= -0.15 + i * 0.15 +random.uniform(-1, 1)*0.03, z=0.7725)))
|
||||
|
||||
"""
|
||||
Pose(position=Point(x=0.60, y=0.1265, z=0.7725)),
|
||||
Pose(position=Point(x=0.80, y=0.12, z=0.7725)),
|
||||
Pose(position=Point(x=0.60, y=-0.1, z=0.7725)),
|
||||
Pose(position=Point(x=0.80, y=-0.1, z=0.7725)),
|
||||
Pose(position=Point(x=0.4225, y=-0.1, z=0.7725)),
|
||||
Pose(position=Point(x=0.60, y=-0.35, z=0.7725)),
|
||||
Pose(position=Point(x=0.80, y=-0.35, z=0.7725)),
|
||||
Pose(position=Point(x=0.4225, y=-0.35, z=0.7725))
|
||||
"""
|
||||
|
||||
for (i, (pose, color_mappings)) in enumerate(zip(block_poses, BLOCK_COLOR_MAPPINGS)):
|
||||
name = "block{}".format(i)
|
||||
|
||||
mappings = {"edge_length" : str(CUBE_EDGE_LENGTH)}
|
||||
mappings.update(color_mappings)
|
||||
|
||||
spawn_xacro_model(name, block_path, pose, world_reference_frame, mappings)
|
||||
model_list.append(name)
|
||||
|
||||
return model_list, block_poses
|
||||
|
||||
|
||||
def delete_gazebo_models(model_list):
|
||||
# This will be called on ROS Exit, deleting Gazebo models
|
||||
# Do not wait for the Gazebo Delete Model service, since
|
||||
# Gazebo should already be running. If the service is not
|
||||
# available since Gazebo has been killed, it is fine to error out
|
||||
try:
|
||||
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
|
||||
|
||||
for model in model_list:
|
||||
resp_delete = delete_model(model)
|
||||
except rospy.ServiceException as e:
|
||||
print("Delete Model service call failed: {0}".format(e))
|
||||
|
|
@ -0,0 +1,281 @@
|
|||
#! /usr/bin/env python
|
||||
|
||||
# Copyright (c) 2016-2018, Rethink Robotics Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
from rospkg.distro import current_distro_codename
|
||||
|
||||
import rospy
|
||||
import argparse
|
||||
from geometry_msgs.msg import Pose
|
||||
from intera_motion_interface import (
|
||||
MotionTrajectory,
|
||||
MotionWaypoint,
|
||||
MotionWaypointOptions,
|
||||
InteractionOptions
|
||||
)
|
||||
from intera_motion_msgs.msg import TrajectoryOptions
|
||||
from intera_interface import Limb
|
||||
from intera_motion_interface.utility_functions import int2bool
|
||||
|
||||
|
||||
def interaction_joint_trajectory(limb,
|
||||
joint_angles,
|
||||
trajType,
|
||||
interaction_active,
|
||||
interaction_control_mode,
|
||||
interaction_frame,
|
||||
speed_ratio,
|
||||
accel_ratio,
|
||||
K_impedance,
|
||||
max_impedance,
|
||||
in_endpoint_frame,
|
||||
force_command,
|
||||
K_nullspace,
|
||||
endpoint_name,
|
||||
timeout,
|
||||
disable_damping_in_force_control,
|
||||
disable_reference_resetting,
|
||||
rotations_for_constrained_zeroG):
|
||||
try:
|
||||
|
||||
traj = MotionTrajectory(limb=limb)
|
||||
|
||||
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
|
||||
max_joint_accel=accel_ratio)
|
||||
|
||||
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
|
||||
|
||||
# one single waypoint
|
||||
current_joint_angles = limb.joint_ordered_angles()
|
||||
waypoint.set_joint_angles(joint_angles = current_joint_angles )
|
||||
traj.append_waypoint(waypoint.to_msg())
|
||||
|
||||
if len(current_joint_angles ) != len(joint_angles):
|
||||
rospy.logerr('The number of joint_angles must be %d', len(current_joint_angles ))
|
||||
return None
|
||||
|
||||
# ----- testing intermediate points with real robot
|
||||
middle_joint_angles = [ (current_joint_angles[i] + joint_angles[i])/2.0 for i in xrange(len(current_joint_angles))]
|
||||
waypoint.set_joint_angles(joint_angles=middle_joint_angles)
|
||||
traj.append_waypoint(waypoint.to_msg())
|
||||
|
||||
waypoint.set_joint_angles(joint_angles = joint_angles)
|
||||
traj.append_waypoint(waypoint.to_msg())
|
||||
# ----- end testing intermediate points with real robot
|
||||
|
||||
# set the interaction control options in the current configuration
|
||||
interaction_options = InteractionOptions()
|
||||
trajectory_options = TrajectoryOptions()
|
||||
trajectory_options.interaction_control = True
|
||||
trajectory_options.interpolation_type = trajType
|
||||
|
||||
interaction_options.set_interaction_control_active(int2bool(interaction_active))
|
||||
interaction_options.set_K_impedance(K_impedance)
|
||||
interaction_options.set_max_impedance(int2bool(max_impedance))
|
||||
interaction_options.set_interaction_control_mode(interaction_control_mode)
|
||||
interaction_options.set_in_endpoint_frame(int2bool(in_endpoint_frame))
|
||||
interaction_options.set_force_command(force_command)
|
||||
interaction_options.set_K_nullspace(K_nullspace)
|
||||
interaction_options.set_endpoint_name(endpoint_name)
|
||||
if len(interaction_frame) < 7:
|
||||
rospy.logerr('The number of elements must be 7!')
|
||||
elif len(interaction_frame) == 7:
|
||||
|
||||
quat_sum_square = interaction_frame[3]*interaction_frame[3] + interaction_frame[4]*interaction_frame[4] + interaction_frame[5]*interaction_frame[5] + interaction_frame[6]*interaction_frame[6]
|
||||
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
|
||||
target_interaction_frame = Pose()
|
||||
target_interaction_frame.position.x = interaction_frame[0]
|
||||
target_interaction_frame.position.y = interaction_frame[1]
|
||||
target_interaction_frame.position.z = interaction_frame[2]
|
||||
target_interaction_frame.orientation.w = interaction_frame[3]
|
||||
target_interaction_frame.orientation.x = interaction_frame[4]
|
||||
target_interaction_frame.orientation.y = interaction_frame[5]
|
||||
target_interaction_frame.orientation.z = interaction_frame[6]
|
||||
interaction_options.set_interaction_frame(target_interaction_frame)
|
||||
else:
|
||||
rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
|
||||
else:
|
||||
rospy.logerr('Invalid input to interaction_frame!')
|
||||
|
||||
interaction_options.set_disable_damping_in_force_control(disable_damping_in_force_control)
|
||||
interaction_options.set_disable_reference_resetting(disable_reference_resetting)
|
||||
interaction_options.set_rotations_for_constrained_zeroG(rotations_for_constrained_zeroG)
|
||||
|
||||
trajectory_options.interaction_params = interaction_options.to_msg()
|
||||
traj.set_trajectory_options(trajectory_options)
|
||||
|
||||
result = traj.send_trajectory(timeout=timeout)
|
||||
if result is None:
|
||||
rospy.logerr('Trajectory FAILED to send!')
|
||||
return
|
||||
|
||||
if result.result:
|
||||
rospy.loginfo('Motion controller successfully finished the trajectory with interaction options set!')
|
||||
else:
|
||||
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
|
||||
result.errorId)
|
||||
|
||||
# print the resultant interaction options
|
||||
rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg())
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
rospy.logerr('Keyboard interrupt detected from the user. %s',
|
||||
'Exiting before trajectory completion.')
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
Move the robot arm to the specified configuration
|
||||
with the desired interaction control options.
|
||||
Call using:
|
||||
$ rosrun intera_examples go_to_joint_angles_in_contact.py [arguments: see below]
|
||||
|
||||
-q 0.0 0.0 0.0 0.0 0.0 0.0 0.0
|
||||
--> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings
|
||||
|
||||
-q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1
|
||||
--> Go to pose [...] with a speed ratio of 0.1
|
||||
|
||||
-q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1
|
||||
--> Go to pose [...] witha speed ratio of 0.9 and an accel ratio of 0.1
|
||||
|
||||
--trajType CARTESIAN
|
||||
--> Use a Cartesian interpolated endpoint path to reach the goal
|
||||
|
||||
=== Interaction Mode options ===
|
||||
|
||||
-st 1
|
||||
--> Set the interaction controller state (1 for True, 0 for False) in the current configuration
|
||||
|
||||
-k 500.0 500.0 500.0 10.0 10.0 10.0
|
||||
--> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration
|
||||
|
||||
-m 0
|
||||
--> Set max_impedance to False for all 6 directions in the current configuration
|
||||
|
||||
-m 1 1 0 1 1 1
|
||||
--> Set max_impedance to [True True False True True True] in the current configuration
|
||||
|
||||
-kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
|
||||
--> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration
|
||||
|
||||
-f 0.0 0.0 30.0 0.0 0.0 0.0
|
||||
--> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration
|
||||
|
||||
-ef
|
||||
--> Set in_endpoint_frame to True in the current configuration
|
||||
|
||||
-en 'right_hand'
|
||||
--> Specify the desired endpoint frame where impedance and force control behaviors are defined
|
||||
|
||||
-md 1
|
||||
--> Set interaction_control_mode to impedance mode for all 6 directions in the current configuration
|
||||
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
|
||||
|
||||
-md 1 1 2 1 1 1
|
||||
--> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance] in the current configuration
|
||||
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
|
||||
"""
|
||||
|
||||
arg_fmt = argparse.RawDescriptionHelpFormatter
|
||||
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
|
||||
description=main.__doc__)
|
||||
parser.add_argument(
|
||||
"-q", "--joint_angles", type=float,
|
||||
nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
|
||||
help="A list of joint angles, one for each of the 7 joints, J0...J6")
|
||||
parser.add_argument(
|
||||
"-s", "--speed_ratio", type=float, default=0.1,
|
||||
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
|
||||
parser.add_argument(
|
||||
"-a", "--accel_ratio", type=float, default=0.5,
|
||||
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
|
||||
parser.add_argument(
|
||||
"-t", "--trajType", type=str, default='JOINT',
|
||||
choices=['JOINT', 'CARTESIAN'],
|
||||
help="trajectory interpolation type")
|
||||
parser.add_argument(
|
||||
"-st", "--interaction_active", type=int, default=1, choices = [0, 1],
|
||||
help="Activate (1) or Deactivate (0) interaction controller")
|
||||
parser.add_argument(
|
||||
"-k", "--K_impedance", type=float,
|
||||
nargs='+', default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0],
|
||||
help="A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values")
|
||||
parser.add_argument(
|
||||
"-m", "--max_impedance", type=int,
|
||||
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [0, 1],
|
||||
help="A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True")
|
||||
parser.add_argument(
|
||||
"-md", "--interaction_control_mode", type=int,
|
||||
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [1,2,3,4],
|
||||
help="A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions")
|
||||
parser.add_argument(
|
||||
"-fr", "--interaction_frame", type=float,
|
||||
nargs='+', default=[0, 0, 0, 1, 0, 0, 0],
|
||||
help="Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)")
|
||||
parser.add_argument(
|
||||
"-ef", "--in_endpoint_frame", action='store_true', default=False,
|
||||
help="Set the desired reference frame to endpoint frame; otherwise, it is base frame by default")
|
||||
parser.add_argument(
|
||||
"-en", "--endpoint_name", type=str, default='right_hand',
|
||||
help="Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default")
|
||||
parser.add_argument(
|
||||
"-f", "--force_command", type=float,
|
||||
nargs='+', default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
help="A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed")
|
||||
parser.add_argument(
|
||||
"-kn", "--K_nullspace", type=float,
|
||||
nargs='+', default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0],
|
||||
help="A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)")
|
||||
parser.add_argument(
|
||||
"-dd", "--disable_damping_in_force_control", action='store_true', default=False,
|
||||
help="Disable damping in force control")
|
||||
parser.add_argument(
|
||||
"-dr", "--disable_reference_resetting", action='store_true', default=False,
|
||||
help="The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature.")
|
||||
parser.add_argument(
|
||||
"-rc", "--rotations_for_constrained_zeroG", action='store_true', default=False,
|
||||
help="Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)")
|
||||
parser.add_argument(
|
||||
"--timeout", type=float, default=None,
|
||||
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
|
||||
|
||||
args = parser.parse_args(rospy.myargv()[1:])
|
||||
|
||||
|
||||
rospy.init_node('go_to_joint_angles_in_contact_py')
|
||||
limb = Limb()
|
||||
|
||||
interaction_joint_trajectory(limb,
|
||||
joint_angles = args.joint_angles,
|
||||
trajType= args.trajType,
|
||||
interaction_control_mode= args.interaction_control_mode,
|
||||
interaction_active= args.interaction_active,
|
||||
interaction_frame= args.interaction_frame,
|
||||
speed_ratio = args.speed_ratio,
|
||||
accel_ratio = args.accel_ratio,
|
||||
K_impedance= args.K_impedance,
|
||||
max_impedance = args.max_impedance,
|
||||
in_endpoint_frame = args.in_endpoint_frame,
|
||||
force_command = args.force_command,
|
||||
K_nullspace = args.K_nullspace,
|
||||
endpoint_name = args.endpoint_name,
|
||||
timeout = args.timeout,
|
||||
disable_damping_in_force_control=args.disable_damping_in_force_control,
|
||||
disable_reference_resetting = args.disable_reference_resetting ,
|
||||
rotations_for_constrained_zeroG = args.rotations_for_constrained_zeroG
|
||||
)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
#!/usr/bin/python
|
||||
import functools
|
||||
import sys
|
||||
|
||||
import rospy
|
||||
import gazebo_models
|
||||
from task_planner import TaskPlanner
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
rospy.init_node("sorting_demo")
|
||||
|
||||
# Load Gazebo Models via Spawning Services
|
||||
# Note that the models reference is the /world frame
|
||||
# and the IK operates with respect to the /base frame
|
||||
model_list, original_block_poses = gazebo_models.load_gazebo_models()
|
||||
# Remove models from the scene on shutdown
|
||||
|
||||
rospy.on_shutdown(functools.partial(gazebo_models.delete_gazebo_models, model_list))
|
||||
|
||||
task_planner = TaskPlanner()
|
||||
|
||||
task_facade = task_planner.get_task_facade()
|
||||
|
||||
task_facade.start()
|
||||
|
||||
task_facade.run_rest_server()
|
||||
|
||||
#rospy.sleep(15)
|
||||
#task_facade.pick_block_by_color("BLUE")
|
||||
#task_facade.put_block_into_tray("BLUE", "1")
|
||||
|
||||
#task_facade.put_all_contents_on_table()
|
||||
|
||||
#task_facade.pause()
|
||||
|
||||
#rospy.sleep(30)
|
||||
#task_facade.resume()
|
||||
|
||||
#task_facade.stop("GREEN")
|
||||
|
||||
rospy.logwarn("task planner spin")
|
||||
task_planner.spin()
|
||||
|
||||
# ask the robot to greet
|
||||
#task_facade.greet()
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
|
|
@ -0,0 +1,218 @@
|
|||
#!/usr/bin/python
|
||||
import copy
|
||||
from datetime import time
|
||||
|
||||
import intera_interface
|
||||
import rospy
|
||||
from geometry_msgs.msg import (
|
||||
Pose,
|
||||
)
|
||||
|
||||
class SawyerRobotControl(object):
|
||||
def __init__(self, limb="right", hover_distance=0.15, tip_name="right_gripper_tip"):
|
||||
"""
|
||||
:param limb:
|
||||
:param hover_distance:
|
||||
:param tip_name:
|
||||
"""
|
||||
self._limb_name = limb # string
|
||||
self._tip_name = tip_name # string
|
||||
self._hover_distance = hover_distance # in meters
|
||||
self._limb = intera_interface.Limb(limb)
|
||||
self._gripper = intera_interface.Gripper()
|
||||
self._robot_enable = intera_interface.RobotEnable()
|
||||
|
||||
# verify robot is enabled
|
||||
print("Getting robot state... ")
|
||||
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
|
||||
self._init_state = self._rs.state().enabled
|
||||
print("Enabling robot... ")
|
||||
self._rs.enable()
|
||||
|
||||
def move_to_start(self, start_angles=None):
|
||||
"""
|
||||
:param start_angles:
|
||||
:return:
|
||||
"""
|
||||
print("Moving the {0} arm to start pose...".format(self._limb_name))
|
||||
if not start_angles:
|
||||
start_angles = dict(zip(self._joint_names, [0] * 7))
|
||||
self._guarded_move_to_joint_position(start_angles)
|
||||
self.gripper_open()
|
||||
|
||||
def pick_loop(self, pose, approach_speed=0.001, approach_time=3.0, meet_time=2.0, retract_time=2.0,
|
||||
hover_distance=None):
|
||||
"""
|
||||
Internal state machine for picking
|
||||
:param pose:
|
||||
:return:
|
||||
"""
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
|
||||
# open the gripper
|
||||
|
||||
self.gripper_open()
|
||||
rospy.sleep(0.1)
|
||||
|
||||
if hover_distance is None:
|
||||
hover_distance = self._hover_distance
|
||||
|
||||
# servo above pose
|
||||
self._approach(pose, time=approach_time, approach_speed=approach_speed, hover_distance=hover_distance)
|
||||
rospy.sleep(1.0)
|
||||
|
||||
# servo to pose
|
||||
self._servo_to_pose_loop(pose, time=meet_time)
|
||||
# rospy.sleep(1.0)
|
||||
|
||||
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
|
||||
# rospy.sleep(1.0)
|
||||
# close gripper
|
||||
self.gripper_close()
|
||||
self._gripper.set_object_weight(0.25)
|
||||
|
||||
rospy.sleep(0.1)
|
||||
|
||||
# retract to clear object
|
||||
self._retract_loop(time=retract_time, hover_distance=hover_distance)
|
||||
|
||||
def place_loop(self, pose, approach_speed, approach_time, meet_time, retract_time):
|
||||
"""
|
||||
Internal state machine for placing
|
||||
:param pose:
|
||||
:return:
|
||||
"""
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# servo above pose
|
||||
self._approach(pose, time=approach_time, approach_speed=approach_speed, hover_distance=self._hover_distance)
|
||||
rospy.sleep(0.1)
|
||||
|
||||
# servo to pose
|
||||
self._servo_to_pose_loop(pose, time=meet_time)
|
||||
rospy.sleep(0.1)
|
||||
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# open the gripper
|
||||
self.gripper_open()
|
||||
rospy.sleep(0.1)
|
||||
|
||||
self.gripper_open()
|
||||
self._gripper.set_object_weight(0)
|
||||
|
||||
rospy.sleep(0.1)
|
||||
# retract to clear object
|
||||
self._retract_loop(time=retract_time)
|
||||
|
||||
def gripper_open(self):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("OPENING GRIPPER")
|
||||
self._gripper.open()
|
||||
while self._gripper.is_moving() and not rospy.is_shutdown():
|
||||
rospy.sleep(0.1)
|
||||
|
||||
def gripper_close(self):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("CLOSING GRIPPER")
|
||||
self._gripper.close()
|
||||
|
||||
while self._gripper.is_moving() and not rospy.is_shutdown():
|
||||
rospy.sleep(0.1)
|
||||
|
||||
def _guarded_move_to_joint_position(self, joint_angles, timeout=5.0):
|
||||
"""
|
||||
:param joint_angles:
|
||||
:param timeout:
|
||||
:return:
|
||||
"""
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
if joint_angles:
|
||||
self._limb.move_to_joint_positions(joint_angles, timeout=timeout)
|
||||
else:
|
||||
rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
|
||||
|
||||
def _approach(self, pose, time, hover_distance, approach_speed=0.001):
|
||||
"""
|
||||
:param pose:
|
||||
:param time:
|
||||
:param approach_speed:
|
||||
:return:
|
||||
"""
|
||||
approach = copy.deepcopy(pose)
|
||||
rospy.logwarn("approach pose:" + str(approach))
|
||||
rospy.logwarn("hover distance:" + str(hover_distance))
|
||||
# approach with a pose the hover-distance above the requested pose
|
||||
|
||||
approach.position.z = approach.position.z + hover_distance
|
||||
joint_angles = self._limb.ik_request(approach, self._tip_name)
|
||||
|
||||
# self._limb.set_joint_position_speed(0.0001)
|
||||
# self._guarded_move_to_joint_position(joint_angles)
|
||||
self._servo_to_pose_loop(approach, time=time)
|
||||
rospy.sleep(0.1)
|
||||
# self._limb.set_joint_position_speed(0.0001)
|
||||
|
||||
def _retract_loop(self, time=2, hover_distance=None):
|
||||
|
||||
if hover_distance is None:
|
||||
hover_distance = self._hover_distance
|
||||
|
||||
# retrieve current pose from endpoint
|
||||
current_pose = self._limb.endpoint_pose()
|
||||
ik_pose = Pose()
|
||||
ik_pose.position.x = current_pose['position'].x
|
||||
ik_pose.position.y = current_pose['position'].y
|
||||
ik_pose.position.z = current_pose['position'].z + hover_distance
|
||||
ik_pose.orientation.x = current_pose['orientation'].x
|
||||
ik_pose.orientation.y = current_pose['orientation'].y
|
||||
ik_pose.orientation.z = current_pose['orientation'].z
|
||||
ik_pose.orientation.w = current_pose['orientation'].w
|
||||
self._servo_to_pose_loop(ik_pose, time=time)
|
||||
|
||||
def _servo_to_pose_loop(self, target_pose, time=4.0, steps=400.0):
|
||||
""" An *incredibly simple* linearly-interpolated Cartesian move """
|
||||
r = rospy.Rate(1 / (time / steps)) # Defaults to 100Hz command rate
|
||||
current_pose = self._limb.endpoint_pose()
|
||||
ik_delta = Pose()
|
||||
ik_delta.position.x = (current_pose['position'].x - target_pose.position.x) / steps
|
||||
ik_delta.position.y = (current_pose['position'].y - target_pose.position.y) / steps
|
||||
ik_delta.position.z = (current_pose['position'].z - target_pose.position.z) / steps
|
||||
ik_delta.orientation.x = (current_pose['orientation'].x - target_pose.orientation.x) / steps
|
||||
ik_delta.orientation.y = (current_pose['orientation'].y - target_pose.orientation.y) / steps
|
||||
ik_delta.orientation.z = (current_pose['orientation'].z - target_pose.orientation.z) / steps
|
||||
ik_delta.orientation.w = (current_pose['orientation'].w - target_pose.orientation.w) / steps
|
||||
for d in range(int(steps), -1, -1):
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
ik_step = Pose()
|
||||
ik_step.position.x = d * ik_delta.position.x + target_pose.position.x
|
||||
ik_step.position.y = d * ik_delta.position.y + target_pose.position.y
|
||||
ik_step.position.z = d * ik_delta.position.z + target_pose.position.z
|
||||
ik_step.orientation.x = d * ik_delta.orientation.x + target_pose.orientation.x
|
||||
ik_step.orientation.y = d * ik_delta.orientation.y + target_pose.orientation.y
|
||||
ik_step.orientation.z = d * ik_delta.orientation.z + target_pose.orientation.z
|
||||
ik_step.orientation.w = d * ik_delta.orientation.w + target_pose.orientation.w
|
||||
joint_angles = self._limb.ik_request(ik_step, self._tip_name)
|
||||
if joint_angles:
|
||||
self._limb.set_joint_positions(joint_angles)
|
||||
else:
|
||||
rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
|
||||
|
||||
r.sleep()
|
||||
r.sleep()
|
||||
|
||||
def disable(self):
|
||||
self._robot_enable.disable()
|
||||
|
||||
def enable(self):
|
||||
self._robot_enable.enable()
|
||||
|
|
@ -0,0 +1,245 @@
|
|||
import flask
|
||||
import rospy
|
||||
from flask import Flask
|
||||
|
||||
|
||||
class RobotTaskFacade:
|
||||
"""
|
||||
class specially designed to control the robot via voice commands. Some examples could be:
|
||||
'Hello robot'
|
||||
'Hey robot! What task are you doing now?'
|
||||
'Hey robot! Put all red pieces on the tray 2'
|
||||
'Hey robot! Pause your work'
|
||||
'Hey robot! Give me a red brick'
|
||||
'Hey robot! Stop sorting green pieces'
|
||||
'Hey robot! restart your work'
|
||||
'Hey robot! What is the color of the piece you are grabbing'
|
||||
'Hey robot! Put all tray contents on the table'
|
||||
"""
|
||||
|
||||
def __init__(self, task_planner):
|
||||
self.task_planner = task_planner
|
||||
|
||||
self.app = Flask(__name__)
|
||||
|
||||
@self.app.route("/state")
|
||||
def get_state():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.get_state())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/current_task")
|
||||
def get_current_task():
|
||||
try:
|
||||
return flask.json.jsonify(response= "ACK", result = self.get_current_task())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response= "ERR", message = ex.message)
|
||||
|
||||
@self.app.route("/count_table_pieces")
|
||||
def count_pieces_on_table_by_color():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.count_pieces_on_table_by_color())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/current_piece_color")
|
||||
def get_current_piece_color():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.get_current_piece_color())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/pause")
|
||||
def pause():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.pause())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/resume")
|
||||
def resume():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.resume())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/start")
|
||||
def start():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.start())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/stop")
|
||||
def stop():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.stop())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/greet")
|
||||
def greet():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.greet())
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/put_all_contents_on_table")
|
||||
def put_all_contents_on_table():
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.put_all_contents_on_table)
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
|
||||
@self.app.route("/pick_block_by_color/<color>")
|
||||
def pick_block_by_color(color):
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.pick_block_by_color(color))
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
@self.app.route("/put_block_into_tray/<color>/<trayid>")
|
||||
def put_block_into_tray( color, trayid):
|
||||
try:
|
||||
return flask.json.jsonify(response="ACK", result=self.put_block_into_tray(color,int(trayid)))
|
||||
except Exception as ex:
|
||||
return flask.json.jsonify(response="ERR", message=ex.message)
|
||||
|
||||
def run_rest_server(self):
|
||||
self.app.run(threaded=True)
|
||||
|
||||
# -------- observer methods ------------------
|
||||
|
||||
def get_state(self):
|
||||
"""
|
||||
|
||||
:return: the state of the application
|
||||
"""
|
||||
|
||||
return self.task_planner.get_state()
|
||||
|
||||
def get_current_task(self):
|
||||
"""
|
||||
:return: str - it gets the name of the task the robot is doing
|
||||
"""
|
||||
# returns the higher level task
|
||||
|
||||
task_stack = self.task_planner.get_task_stack()
|
||||
|
||||
return task_stack
|
||||
|
||||
def count_pieces_on_table_by_color(self):
|
||||
"""
|
||||
:param color: str - "red", "blue", "green"
|
||||
:return:
|
||||
"""
|
||||
rospy.logerr("To implement. Robot Task: count pieces on table by color!")
|
||||
return 0
|
||||
|
||||
def get_current_piece_color(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
|
||||
rospy.logerr("To implement. Robot Task: get current piece color!")
|
||||
return "GREEN"
|
||||
|
||||
# ---------- lifecycle ---------------------------
|
||||
def pause(self):
|
||||
"""
|
||||
'Hey robot! Stop your work'
|
||||
|
||||
It pauses the loop job
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.pause()
|
||||
return "ACK"
|
||||
|
||||
def resume(self):
|
||||
"""
|
||||
resume the loop job
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.resume()
|
||||
return "ACK"
|
||||
|
||||
def start(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.create_main_loop_task()
|
||||
return "ACK"
|
||||
|
||||
def stop(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.stop()
|
||||
return "ACK"
|
||||
|
||||
# --------- request tasks methods ---------------------
|
||||
|
||||
def greet(self):
|
||||
"""
|
||||
'Hello robot'
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.execute_task(self.task_planner.create_greet_task)
|
||||
return "ACK"
|
||||
|
||||
def put_all_contents_on_table(self):
|
||||
"""
|
||||
'Hey robot! Put all red pieces on the tray 2'
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.execute_task(self.task_planner.put_all_contents_on_table)
|
||||
return "ACK"
|
||||
|
||||
def pick_block_by_color(self, color):
|
||||
"""
|
||||
'Hey robot! Give me a red brick'
|
||||
|
||||
|
||||
:param color: str - "red", "blue", "green"
|
||||
:return:
|
||||
"""
|
||||
|
||||
self.task_planner.execute_task(self.task_planner.pick_block_on_table_by_color, args=[color])
|
||||
return "ACK"
|
||||
|
||||
|
||||
def put_block_into_tray(self, color, trayid):
|
||||
"""
|
||||
'Hey robot! Put a red piece on the tray 2'
|
||||
|
||||
:param color: str - "red", "blue", "green"
|
||||
:param tray:
|
||||
:return:
|
||||
"""
|
||||
|
||||
self.task_planner.execute_task(self.task_planner.put_block_into_tray_task, args=[color,trayid])
|
||||
return "ACK"
|
||||
|
||||
# ------------ configure loop sorting ---------------------
|
||||
#@route("/disable_sorting_by_color/<color>')")
|
||||
def disable_sorting_by_color(self, color):
|
||||
"""
|
||||
'Hey robot! Stop sorting green pieces'
|
||||
:param color:
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.disable_sorting_by_color(color)
|
||||
|
||||
#@route("/enable_sorting_by_color/<color>')")
|
||||
def enable_sorting_by_color(self,color):
|
||||
"""
|
||||
'Hey robot! Stop sorting green pieces'
|
||||
:param color:
|
||||
:return:
|
||||
"""
|
||||
self.task_planner.enable_sorting_by_color(color)
|
||||
|
|
@ -0,0 +1,803 @@
|
|||
#!/usr/bin/python
|
||||
import copy
|
||||
import math
|
||||
import numpy
|
||||
import traceback
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from functools import wraps
|
||||
from threading import Lock
|
||||
|
||||
import moveit_msgs.msg
|
||||
import moveit_msgs.srv
|
||||
import rospy
|
||||
from geometry_msgs.msg import Quaternion
|
||||
|
||||
import demo_constants
|
||||
import utils.mathutils
|
||||
from environment_estimation import EnvironmentEstimation
|
||||
from robot_control import SawyerRobotControl
|
||||
from robot_tasks_facade import RobotTaskFacade
|
||||
|
||||
import time
|
||||
from re import search
|
||||
from functools import wraps
|
||||
|
||||
|
||||
def tasync(taskname):
|
||||
def wrapper(f):
|
||||
@wraps(f)
|
||||
def wrapped(self, *f_args, **f_kwargs):
|
||||
|
||||
if self.has_cancel_signal():
|
||||
rospy.logerr("trying to invoke but cancel signal: " + str(taskname))
|
||||
self.print_tasks()
|
||||
return Task("CANCEL", None)
|
||||
|
||||
if self.pause_flag:
|
||||
rospy.logerr("PAUSEEEE")
|
||||
while self.pause_flag and not rospy.is_shutdown():
|
||||
rospy.sleep(0.5)
|
||||
rospy.logwarn("Task %s is paused" % taskname)
|
||||
|
||||
tt = Task(taskname, None)
|
||||
|
||||
def lamb():
|
||||
res = None
|
||||
try:
|
||||
# f_kwargs["task"] = tt
|
||||
res = f(self, *f_args, **f_kwargs)
|
||||
except Exception as ex:
|
||||
rospy.logerr("task wrapping error (%s): %s" % (taskname, str(ex)))
|
||||
traceback.print_exc()
|
||||
return res
|
||||
|
||||
self.add_task(tt)
|
||||
|
||||
fut = self.executor.submit(lamb)
|
||||
tt.future = fut
|
||||
|
||||
def got_result(fut):
|
||||
try:
|
||||
rospy.logwarn("removing task: " + tt.name)
|
||||
self.remove_task(tt)
|
||||
except Exception as ex:
|
||||
rospy.logwarn("error at done callback: " + tt.name + str(ex))
|
||||
|
||||
self.print_tasks()
|
||||
|
||||
fut.add_done_callback(got_result)
|
||||
|
||||
return tt
|
||||
|
||||
return wrapped
|
||||
|
||||
return wrapper
|
||||
|
||||
|
||||
class Task:
|
||||
def __init__(self, name, future):
|
||||
self.name = name
|
||||
self.future = future
|
||||
self.marked_cancel = False
|
||||
|
||||
def cancel(self):
|
||||
marked_cancel = True
|
||||
resultcancel = self.future.cancel()
|
||||
|
||||
def result(self):
|
||||
if self.future is not None:
|
||||
return self.future.result()
|
||||
else:
|
||||
return None
|
||||
|
||||
|
||||
class TaskPlanner:
|
||||
def __init__(self):
|
||||
"""
|
||||
"""
|
||||
limb = 'right'
|
||||
hover_distance = 0.2 # meters
|
||||
|
||||
# subcomponents
|
||||
self.environment_estimation = EnvironmentEstimation()
|
||||
self.sawyer_robot = SawyerRobotControl(limb, hover_distance)
|
||||
|
||||
self.tasks = []
|
||||
self.executor = ThreadPoolExecutor(max_workers=4)
|
||||
|
||||
self.cancel_signal = False
|
||||
self.pause_flag = False
|
||||
|
||||
self.mutex = Lock()
|
||||
|
||||
self.current_in_hand_block = None
|
||||
self.current_in_hand_block_target_tray = None
|
||||
|
||||
self.task_facade = RobotTaskFacade(self)
|
||||
|
||||
def has_cancel_signal(self):
|
||||
return self.cancel_signal
|
||||
|
||||
def add_task(self, task):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("adding task: " + task.name)
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
self.tasks.append(task)
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def remove_task(self, task):
|
||||
"""
|
||||
|
||||
:param tt:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
self.tasks.remove(task)
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def get_task_facade(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
return self.task_facade
|
||||
|
||||
def robot_sayt2s(self, text):
|
||||
rospy.logwarn("ROBOT SAYS: " + text)
|
||||
|
||||
@tasync("MOVE XY")
|
||||
def create_move_to_xyz_pr(self, target_pose):
|
||||
"""
|
||||
Uses de 5DOF ik to locate the arm on top of the table with the camera looking ortogonally to the table.
|
||||
The free parameter is the yaw (rotation on Z axis)
|
||||
:param target_pose:
|
||||
:return:
|
||||
"""
|
||||
|
||||
rospy.logwarn("CALLING IK SERVICE")
|
||||
ikservice = rospy.ServiceProxy("/sawyer_ik_5d_node/ik", moveit_msgs.srv.GetPositionIK)
|
||||
|
||||
ik_req = moveit_msgs.msg.PositionIKRequest()
|
||||
ik_req.robot_state.joint_state.name = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5",
|
||||
"right_j6"]
|
||||
|
||||
jntangles = self.sawyer_robot._limb.joint_angles()
|
||||
ik_req.robot_state.joint_state.position = [jntangles[k] for k in jntangles]
|
||||
ik_req.pose_stamped.pose = target_pose
|
||||
# ik_req.constraints.ik_link_name = "right_hand_camera_optical"
|
||||
|
||||
rospy.logwarn("CALLING IK SERVICE request: " + str(ik_req))
|
||||
resp = ikservice(ik_req)
|
||||
|
||||
rospy.logwarn("SERVICE RESPONSE:" + str(resp))
|
||||
|
||||
targetjoints = dict(zip(resp.solution.joint_state.name, resp.solution.joint_state.position))
|
||||
|
||||
self.sawyer_robot._limb.set_joint_position_speed(0.000001)
|
||||
self.sawyer_robot._guarded_move_to_joint_position(targetjoints)
|
||||
|
||||
def create_pick_tray_task(self, tray, approach_speed, approach_time, meet_time, retract_time):
|
||||
"""
|
||||
"""
|
||||
tray = copy.deepcopy(tray)
|
||||
|
||||
p = copy.deepcopy(tray.get_tray_place_block_location())
|
||||
|
||||
rospy.logwarn("PICK TRAY FINAL POSE: " + str(p))
|
||||
|
||||
return self.create_pick_task(p,
|
||||
approach_speed,
|
||||
approach_time,
|
||||
meet_time,
|
||||
retract_time)
|
||||
|
||||
@tasync("GO HOME")
|
||||
def create_go_home_task(self):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
rospy.loginfo("GO TO HOME TASK")
|
||||
|
||||
# Starting Joint angles for right arm
|
||||
starting_joint_angles = {'right_j0': -0.041662954890248294,
|
||||
'right_j1': -1.0258291091425074,
|
||||
'right_j2': 0.0293680414401436,
|
||||
'right_j3': 2.17518162913313,
|
||||
'right_j4': -0.06703022873354225,
|
||||
'right_j5': 0.3968371433926965,
|
||||
'right_j6': 1.7659649178699421}
|
||||
|
||||
self.sawyer_robot.move_to_start(starting_joint_angles)
|
||||
|
||||
@tasync("GREET TASK")
|
||||
def create_greet_task(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
|
||||
self.create_go_home_task().result()
|
||||
joint_angles_A = {'right_j0': 0.0,
|
||||
'right_j1': 0.0,
|
||||
'right_j2': 0.0,
|
||||
'right_j3': -numpy.pi / 2.0,
|
||||
'right_j4': -numpy.pi / 4.0,
|
||||
'right_j5': 0.0,
|
||||
'right_j6': 0.0}
|
||||
|
||||
joint_angles_B = {'right_j0': 0.0,
|
||||
'right_j1': 0.0,
|
||||
'right_j2': 0.0,
|
||||
'right_j3': -numpy.pi / 2.0,
|
||||
'right_j4': -3 * numpy.pi / 4.0,
|
||||
'right_j5': 0.0,
|
||||
'right_j6': 0.0}
|
||||
|
||||
for i in xrange(4):
|
||||
self.sawyer_robot._guarded_move_to_joint_position(joint_angles_A)
|
||||
self.sawyer_robot._guarded_move_to_joint_position(joint_angles_B)
|
||||
|
||||
@tasync("GO TO VISION POSE")
|
||||
def create_go_vision_head_pose_task(self):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
# joint_angles = self.sawyer_robot._limb.joint_angles()
|
||||
|
||||
joint_angles = {'right_j0': 0.0,
|
||||
'right_j1': -numpy.pi / 2.0,
|
||||
'right_j2': 0.0,
|
||||
'right_j3': 0.0,
|
||||
'right_j4': 0.0,
|
||||
'right_j5': 0.0,
|
||||
'right_j6': 0.0}
|
||||
|
||||
self.sawyer_robot._limb.move_to_joint_positions(joint_angles)
|
||||
self.delay_task(1).result()
|
||||
|
||||
@tasync("TURN OVER TRAY")
|
||||
def create_complete_turn_over_tray(self, target_tray, homepose):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
|
||||
# PICK TRAY
|
||||
self.create_pick_task(copy.deepcopy(target_tray.get_tray_pick_location()),
|
||||
approach_speed=0.0001,
|
||||
approach_time=1.0,
|
||||
meet_time=0.1,
|
||||
retract_time=0.1,
|
||||
hover_distance=0.25).result()
|
||||
|
||||
turnoverpose = copy.deepcopy(homepose)
|
||||
turnoverpose.position.x += 0
|
||||
turnoverpose.position.y -= 0
|
||||
turnoverpose.position.z += 0
|
||||
|
||||
# TARGET POSITION: PRE-TURN OVER
|
||||
self.create_approach_task(turnoverpose,
|
||||
approach_speed=0.0001,
|
||||
approach_time=3.0,
|
||||
hover_distance=0.15).result()
|
||||
|
||||
self.create_turn_over_tray_task(turnoverpose).result()
|
||||
|
||||
self.delay_task(3).result()
|
||||
|
||||
self.create_approach_task(turnoverpose,
|
||||
approach_speed=0.0001,
|
||||
approach_time=3.0,
|
||||
hover_distance=0.15).result()
|
||||
|
||||
self.create_place_task(copy.deepcopy(target_tray.get_tray_pick_location()),
|
||||
approach_speed=0.0001,
|
||||
approach_time=3.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0).result()
|
||||
|
||||
@tasync("TURN OVER TRAY")
|
||||
def create_turn_over_tray_task(self, homepose):
|
||||
"""
|
||||
:param homepose:
|
||||
:return:
|
||||
"""
|
||||
# rotate in y
|
||||
|
||||
reverseTransformY = utils.mathutils.rot_y(-1.3 * numpy.pi / 2.0)
|
||||
# reverseTransformY = utils.mathutils.rot_x(numpy.pi / 2.0)
|
||||
reverseTransformZ = numpy.eye(4)
|
||||
# reverseTransformZ = utils.mathutils.rot_z(numpy.pi)
|
||||
|
||||
joint_angles = self.sawyer_robot._limb.joint_angles()
|
||||
rospy.logwarn("JOINT ANGLES ON TRAY TURN OVER: " + str(joint_angles))
|
||||
joint_angles["right_j6"] -= math.pi
|
||||
|
||||
# prev = self.sawyer_robot._limb.get_joint_position_speed()
|
||||
self.sawyer_robot._limb.set_joint_position_speed(0.0001)
|
||||
self.sawyer_robot._limb.move_to_joint_positions(joint_angles, timeout=15.0)
|
||||
|
||||
self.delay_task(1).result()
|
||||
|
||||
joint_angles["right_j5"] -= 3 * math.pi / 4.0
|
||||
|
||||
# prev = self.sawyer_robot._limb.get_joint_position_speed()
|
||||
self.sawyer_robot._limb.move_to_joint_positions(joint_angles, timeout=15.0)
|
||||
self.delay_task(1).result()
|
||||
|
||||
"""
|
||||
reverseTransform = utils.mathutils.composition(reverseTransformY,reverseTransformZ)
|
||||
|
||||
homehomopose = utils.mathutils.get_homo_matrix_from_pose_msg(homepose)
|
||||
reversedhompose = numpy.matmul(homehomopose, reverseTransform)
|
||||
reversedhome = utils.mathutils.homotransform_to_pose_msg(reversedhompose)
|
||||
|
||||
# move tray on table top
|
||||
return self.create_approach_task(reversedhome,
|
||||
approach_speed=0.0001,
|
||||
approach_time=1.0,
|
||||
hover_distance=0.1)
|
||||
"""
|
||||
|
||||
@tasync("APPROACH")
|
||||
def create_approach_task(self, target_pose, approach_speed, approach_time, hover_distance=None):
|
||||
"""
|
||||
:param target_pose:
|
||||
:param approach_speed:
|
||||
:param approach_time:
|
||||
:param hover_distance:
|
||||
:return:
|
||||
"""
|
||||
self.sawyer_robot._approach(target_pose, approach_time, hover_distance, approach_speed)
|
||||
|
||||
@tasync("PICK")
|
||||
def create_pick_task(self, target_pose, approach_speed, approach_time, meet_time, retract_time,
|
||||
hover_distance):
|
||||
"""
|
||||
:param target_pose:
|
||||
:param approach_speed:
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("PICKING")
|
||||
return self.sawyer_robot.pick_loop(target_pose, approach_speed, approach_time, meet_time, retract_time,
|
||||
hover_distance)
|
||||
|
||||
@tasync("PLACE")
|
||||
def create_place_task(self, target_pose, approach_speed, approach_time, meet_time, retract_time):
|
||||
"""
|
||||
:param target_pose:
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("\nPlacing task..." + str(target_pose))
|
||||
return self.sawyer_robot.place_loop(target_pose, approach_speed, approach_time, meet_time, retract_time)
|
||||
|
||||
@tasync("SELECT BLOCK&TRAY")
|
||||
def create_decision_select_block_and_tray(self, blocks, target_block_index):
|
||||
"""
|
||||
:return:
|
||||
"""
|
||||
|
||||
# An orientation for gripper fingers to be overhead and parallel to the obj
|
||||
|
||||
rospy.logwarn("NEW TARGET BLOCK INDEX: %d" % target_block_index)
|
||||
|
||||
target_block = None
|
||||
if blocks is not None and len(blocks) > 0:
|
||||
target_block = blocks[target_block_index] # access first item , pose field
|
||||
|
||||
target_block.final_pose = self.compute_block_pick_offset_transform(target_block.final_pose)
|
||||
else:
|
||||
rospy.logwarn("OUPS!!")
|
||||
return
|
||||
|
||||
target_tray = self.environment_estimation.get_tray_by_color(target_block.get_color())
|
||||
target_tray.final_pose = self.compute_tray_pick_offset_transform(target_tray.final_pose)
|
||||
|
||||
rospy.logwarn("TARGET TRAY POSE: " + str(target_tray))
|
||||
return target_block, target_tray
|
||||
|
||||
def compute_block_pick_offset_transform(self, pose):
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167,
|
||||
y=0.999994209902,
|
||||
z=-0.00177030764765,
|
||||
w=0.00253311793936)
|
||||
|
||||
overhead_translation = [0,
|
||||
0,
|
||||
-0.25 * demo_constants.CUBE_EDGE_LENGTH]
|
||||
|
||||
pose.orientation = overhead_orientation
|
||||
|
||||
pose.position.x += overhead_translation[0]
|
||||
pose.position.y += overhead_translation[1]
|
||||
pose.position.z += overhead_translation[2]
|
||||
return pose
|
||||
|
||||
def compute_tray_pick_offset_transform(self, pose):
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167,
|
||||
y=0.999994209902,
|
||||
z=-0.00177030764765,
|
||||
w=0.00253311793936)
|
||||
|
||||
pose.orientation = overhead_orientation
|
||||
return pose
|
||||
|
||||
@tasync("SLEEP")
|
||||
def delay_task(self, secs):
|
||||
"""
|
||||
|
||||
:param secs:
|
||||
:return:
|
||||
"""
|
||||
if not rospy.is_shutdown():
|
||||
rospy.sleep(secs)
|
||||
|
||||
@tasync("PICK BLOCK FROM TABLE AND MOVE TO TRAY")
|
||||
def pick_block_on_table_and_place_on_tray(self, target_block, target_tray):
|
||||
"""
|
||||
|
||||
:param original_block_poses:
|
||||
:return:
|
||||
"""
|
||||
rospy.logwarn("target block: " + str(target_block))
|
||||
|
||||
original_block_pose = copy.deepcopy(target_block.final_pose)
|
||||
|
||||
rospy.logwarn("target block pose : " + str(original_block_pose))
|
||||
|
||||
self.create_pick_task(original_block_pose, approach_speed=0.0001, approach_time=2.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0,
|
||||
hover_distance=None).result()
|
||||
|
||||
self.create_place_task(copy.deepcopy(target_tray.get_tray_place_block_location()),
|
||||
approach_speed=0.0001,
|
||||
approach_time=2.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0).result()
|
||||
|
||||
return original_block_pose
|
||||
|
||||
@tasync("BLOCK FROM TRAY TO TABLE")
|
||||
def pick_all_pieces_from_tray_and_put_on_table(self, original_block_pose):
|
||||
"""
|
||||
Pick a block from where it is located on the tray and move it back to the table
|
||||
:param original_block_pose:
|
||||
:return:
|
||||
"""
|
||||
|
||||
self.environment_estimation.update()
|
||||
blocks = self.environment_estimation.get_blocks()
|
||||
blocks_count = len(blocks)
|
||||
target_block_index = 0
|
||||
|
||||
while target_block_index < blocks_count:
|
||||
target_block, target_tray = self.create_detect_block_poses_task(blocks, target_block_index).result()
|
||||
|
||||
self.create_pick_task(copy.deepcopy(target_block.final_pose),
|
||||
approach_speed=0.0001,
|
||||
approach_time=2.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0,
|
||||
hover_distance=None).result()
|
||||
|
||||
place_pose = self.compute_block_pick_offset_transform(original_block_pose[target_block_index])
|
||||
# rospy.logerr("place vs: "+ str(target_block.final_pose) +"\n"+ str(place_pose))
|
||||
|
||||
self.create_place_task(copy.deepcopy(place_pose),
|
||||
approach_speed=0.0001,
|
||||
approach_time=2.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0).result()
|
||||
|
||||
target_block_index += 1
|
||||
|
||||
@tasync("HEAD VISION PROCESSING")
|
||||
def create_head_vision_processing(self):
|
||||
|
||||
self.environment_estimation.update()
|
||||
self.environment_estimation.compute_block_pose_estimations_from_head_camera()
|
||||
|
||||
blocks = self.environment_estimation.get_blocks()
|
||||
|
||||
return blocks
|
||||
|
||||
@tasync("LOCATE ARMVIEW TO BLOCK ESTIMATION")
|
||||
def create_move_top_block_view(self, block):
|
||||
p = copy.deepcopy(block.headview_pose_estimation)
|
||||
# chose z plane
|
||||
p.position.z = 0.1
|
||||
|
||||
poseaux = p # Pose(position=Point(x=0.5 + ki*0.1, y=0.0, z=0.2),orientation=Quaternion(x=0, y=0, z=0, w=1))
|
||||
poseauxhomo = utils.mathutils.get_homo_matrix_from_pose_msg(poseaux)
|
||||
poseauxhomo = utils.mathutils.composition(poseauxhomo, utils.mathutils.rot_y(math.pi / 2.0))
|
||||
poseaux = utils.mathutils.homotransform_to_pose_msg(poseauxhomo)
|
||||
|
||||
self.create_move_to_xyz_pr(poseaux).result()
|
||||
# individual processing algorithm
|
||||
|
||||
self.delay_task(3).result()
|
||||
|
||||
@tasync("OBSERVE ALL CUBES")
|
||||
def create_visit_all_cubes_armview(self, iterations_count=None):
|
||||
""""
|
||||
the robot camera locates on top of each block iteratively and in a loop
|
||||
"""
|
||||
blocks = self.create_head_vision_processing().result()
|
||||
|
||||
iteration = 0
|
||||
while iterations_count is None or iteration < iterations_count:
|
||||
for block in blocks:
|
||||
self.create_move_top_block_view(block).result()
|
||||
|
||||
iteration += 1
|
||||
|
||||
@tasync("WAIT FOREVER")
|
||||
def create_wait_forever_task(self):
|
||||
"""
|
||||
locks the taskplanner forever
|
||||
:return:
|
||||
"""
|
||||
while not rospy.is_shutdown():
|
||||
self.delay_task(10).result()
|
||||
|
||||
@tasync("PICK BY COLOR")
|
||||
def pick_block_on_table_by_color(self, color):
|
||||
|
||||
blocks = self.environment_estimation.table.get_blocks()
|
||||
btarget = [i for i, b in enumerate(blocks) if b.is_color(color)][0]
|
||||
|
||||
target_block, target_tray = self.create_detect_block_poses_task(blocks, btarget).result()
|
||||
|
||||
self.create_pick_task(target_block.final_pose, approach_speed=0.0001, approach_time=2.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0,
|
||||
hover_distance=None).result()
|
||||
|
||||
self.current_in_hand_block = target_block
|
||||
self.current_in_hand_block_target_tray = target_tray
|
||||
|
||||
# self.create_pick_task(btarget.final_pose)
|
||||
return target_block
|
||||
|
||||
@tasync("PICK BY COLOR AND PUT TRAY")
|
||||
def put_block_into_tray_task(self, color, trayid):
|
||||
"""
|
||||
|
||||
:param color:
|
||||
:param trayid:
|
||||
:return:
|
||||
"""
|
||||
#self.reset_cycle()
|
||||
#decide and select block and pick it
|
||||
target_block = self.pick_block_on_table_by_color(color).result()
|
||||
|
||||
rospy.logwarn("put block into tray: " + str(self.environment_estimation.get_trays()))
|
||||
target_tray = self.environment_estimation.get_tray_by_num(trayid)
|
||||
rospy.logwarn("put block into tray: " + str(target_tray))
|
||||
|
||||
place_location = self.compute_block_pick_offset_transform(copy.deepcopy(target_tray.get_tray_place_block_location()))
|
||||
self.create_place_task(place_location,
|
||||
approach_speed=0.0001,
|
||||
approach_time=3.0,
|
||||
meet_time=3.0,
|
||||
retract_time=1.0).result()
|
||||
|
||||
self.environment_estimation.table.notify_block_removed(target_block)
|
||||
target_tray.notify_contains_block(target_block)
|
||||
|
||||
@tasync("REQUEST PUT ALL CONTENTS ON TABLE")
|
||||
def put_all_contents_on_table(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
self.environment_estimation.update()
|
||||
original_blocks_poses = self.environment_estimation.get_original_block_poses()
|
||||
rospy.logwarn(original_blocks_poses)
|
||||
self.pick_all_pieces_from_tray_and_put_on_table(original_blocks_poses).result()
|
||||
self.create_wait_forever_task().result()
|
||||
|
||||
@tasync("DETECT BLOCK POSE")
|
||||
def create_detect_block_poses_task(self, blocks, target_block_index):
|
||||
"""
|
||||
|
||||
:param target_block_index:
|
||||
:return:
|
||||
"""
|
||||
target_block = None
|
||||
target_tray = None
|
||||
while target_block is None:
|
||||
target_block, target_tray = self.create_decision_select_block_and_tray(blocks, target_block_index).result()
|
||||
self.delay_task(0.1).result()
|
||||
|
||||
return target_block, target_tray
|
||||
|
||||
@tasync("MOVE ALL CUBES TO TRAY")
|
||||
def create_move_all_cubes_to_trays(self, blocks):
|
||||
"""
|
||||
Moves all cubes on the table to the trays according with its color
|
||||
:return:
|
||||
"""
|
||||
|
||||
blocks_count = len(blocks)
|
||||
|
||||
target_block_index = 0
|
||||
original_block_poses = []
|
||||
while target_block_index < blocks_count:
|
||||
self.create_go_home_task().result()
|
||||
target_block, target_tray = self.create_detect_block_poses_task(blocks, target_block_index).result()
|
||||
|
||||
self.create_move_top_block_view(target_block).result()
|
||||
|
||||
rospy.logwarn(" -- NEW TARGET BLOCK INDEX: %d" % target_block_index)
|
||||
|
||||
# concurrency issue, what if we lock the objectdetection update?
|
||||
|
||||
original_block_pose = self.pick_block_on_table_and_place_on_tray(target_block, target_tray).result()
|
||||
|
||||
original_block_poses.append(original_block_pose)
|
||||
|
||||
# concurrency issue
|
||||
self.environment_estimation.get_tray(target_tray.id).notify_contains_block(target_block)
|
||||
self.environment_estimation.table.notify_block_removed(target_block)
|
||||
|
||||
target_block_index += 1
|
||||
rospy.logwarn("target block index: " + str(target_block_index))
|
||||
|
||||
return original_block_poses
|
||||
|
||||
@tasync("DISABLE ROBOT")
|
||||
def disable_robot_task(self):
|
||||
self.sawyer_robot.disable()
|
||||
|
||||
@tasync("ENABLE ROBOT")
|
||||
def enable_robot_task(self):
|
||||
self.sawyer_robot.enable()
|
||||
|
||||
@tasync("LOOP SORTING TASK")
|
||||
def create_main_loop_task(self):
|
||||
"""
|
||||
This is the main plan of the application
|
||||
:return:
|
||||
"""
|
||||
self.create_go_home_task().result()
|
||||
|
||||
"""
|
||||
home_position = self.sawyer_robot._limb.endpoint_pose()
|
||||
pos = home_position["position"]
|
||||
q = home_position["orientation"]
|
||||
homepose = Pose(position=Point(x=pos.x, y=pos.y, z=pos.z),
|
||||
orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]))
|
||||
|
||||
rospy.logwarn("home pose:" + str(homepose))
|
||||
"""
|
||||
|
||||
# for ki in xrange(5):
|
||||
# self.create_greet_task().result()
|
||||
|
||||
|
||||
# self.create_go_home_task().result()
|
||||
|
||||
# self.create_visit_all_cubes_armview(1).result()
|
||||
|
||||
|
||||
for i in xrange(2):
|
||||
self.create_go_vision_head_pose_task().result()
|
||||
|
||||
blocks = self.create_head_vision_processing().result()
|
||||
|
||||
self.create_go_home_task().result()
|
||||
|
||||
# self.create_complete_turn_over_tray, target_tray {"homepose": homepose}).result()
|
||||
|
||||
# yield self.create_go_home_task()
|
||||
#self.reset_cycle()
|
||||
# continue
|
||||
|
||||
original_block_poses = self.create_move_all_cubes_to_trays(blocks).result()
|
||||
|
||||
#self.reset_cycle()
|
||||
|
||||
self.pick_all_pieces_from_tray_and_put_on_table(original_block_poses).result()
|
||||
|
||||
self.delay_task(10).result()
|
||||
|
||||
self.create_wait_forever_task().result()
|
||||
|
||||
def reset_cycle(self):
|
||||
for tray in self.environment_estimation.get_trays():
|
||||
tray.reset()
|
||||
|
||||
def get_state(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
return {"table_state": self.environment_estimation.table.get_state(),
|
||||
"trays": [t.get_state() for t in self.environment_estimation.trays],
|
||||
"current_task": self.get_task_stack()}
|
||||
|
||||
def get_task_stack(self):
|
||||
return [t.name for t in self.tasks]
|
||||
|
||||
def execute_task(self, fn, args=[]):
|
||||
"""
|
||||
|
||||
:param fn:
|
||||
:return:
|
||||
"""
|
||||
# INTERRUPT HERE CURRENT TASK AND SAVE STATE
|
||||
|
||||
self.stop()
|
||||
|
||||
fn(*args).result()
|
||||
|
||||
self.robot_sayt2s("REQUESTED TASK COMPLETED")
|
||||
|
||||
self.delay_task(1)
|
||||
|
||||
# self.create_main_loop_task().result()
|
||||
|
||||
def pause(self):
|
||||
rospy.logwarn("PAUSING TASK PLANNER")
|
||||
self.pause_flag = True
|
||||
|
||||
def resume(self):
|
||||
rospy.logwarn("RESUMING TASK PLANNER")
|
||||
self.pause_flag = False
|
||||
|
||||
def stop(self):
|
||||
self.sawyer_robot.disable()
|
||||
self.sawyer_robot.enable()
|
||||
|
||||
# self.disable_robot_task().result()
|
||||
# self.enable_robot_task().result()
|
||||
|
||||
self.cancel_signal = True
|
||||
|
||||
rospy.logwarn("cancelling all tasks")
|
||||
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
for task in self.tasks:
|
||||
rospy.logwarn("cancelling task: " + task.name)
|
||||
task.cancel()
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
# wait until all tasks are finished
|
||||
while len(self.tasks) > 0 and not rospy.is_shutdown():
|
||||
rospy.sleep(0.2)
|
||||
|
||||
self.print_tasks()
|
||||
|
||||
self.cancel_signal = False
|
||||
|
||||
def print_tasks(self):
|
||||
try:
|
||||
self.mutex.acquire()
|
||||
tasksstr = "\n".join([str(t.name) for t in self.tasks])
|
||||
rospy.logwarn("tasks stack: \n" + tasksstr)
|
||||
except Exception as ex:
|
||||
rospy.logerr("error printing task stack: " + str(ex))
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def spin(self):
|
||||
"""
|
||||
|
||||
:return:
|
||||
"""
|
||||
while not rospy.is_shutdown():
|
||||
self.environment_estimation.update()
|
||||
self.print_tasks()
|
||||
rospy.sleep(2.0)
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
import tf
|
||||
import tf.transformations
|
||||
import numpy
|
||||
import geometry_msgs.msg
|
||||
from geometry_msgs.msg import Pose, Point, Quaternion
|
||||
|
||||
|
||||
def inverse_compose(basehomopose, homopose):
|
||||
"""
|
||||
:param basehomopose: 4x4 homogeneous transform matrix
|
||||
:param homopose: 4x4 homogeneous transform matrix
|
||||
:return:
|
||||
"""
|
||||
return numpy.matmul(numpy.linalg.inv(basehomopose), homopose)
|
||||
|
||||
|
||||
def get_homo_matrix_from_pose_msg(pose, tag=""):
|
||||
basetrans = tf.transformations.translation_matrix((pose.position.x,
|
||||
pose.position.y,
|
||||
pose.position.z))
|
||||
|
||||
baserot = tf.transformations.quaternion_matrix((pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w))
|
||||
|
||||
# rospy.loginfo(tag + " basetrans: " + str(basetrans))
|
||||
# rospy.loginfo(tag +" baserot: " + str(baserot))
|
||||
|
||||
combined = numpy.matmul(basetrans, baserot)
|
||||
|
||||
# rospy.loginfo(tag + " combined: " + str(combined))
|
||||
trans = tf.transformations.translation_from_matrix(combined)
|
||||
quat = tf.transformations.quaternion_from_matrix(combined)
|
||||
|
||||
# rospy.loginfo(tag + " back basetrans: " + str(trans))
|
||||
# rospy.loginfo(tag +" back baserot: " + str(quat))
|
||||
|
||||
return combined
|
||||
|
||||
|
||||
def homotransform_to_pose_msg(homotransform):
|
||||
trans = tf.transformations.translation_from_matrix(homotransform)
|
||||
quat = tf.transformations.quaternion_from_matrix(homotransform)
|
||||
return Pose(
|
||||
position=Point(x=trans[0], y=trans[1], z=trans[2]),
|
||||
orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]))
|
||||
|
||||
def composition(parent, child):
|
||||
return numpy.matmul(parent,child)
|
||||
|
||||
def rot_y(pitch):
|
||||
return [[numpy.cos(pitch), 0, numpy.sin(pitch), 0], [0, 1, 0, 0], [-numpy.sin(pitch), 0, numpy.cos(pitch), 0], [0, 0, 0, 1]]
|
||||
|
||||
def rot_z(yaw):
|
||||
return [[numpy.cos(yaw), -numpy.sin(yaw), 0, 0], [numpy.sin(yaw), numpy.cos(yaw), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
|
||||
|
||||
def rot_x(roll):
|
||||
return [[1, 0, 0, 0], [0, numpy.cos(roll), -numpy.sin(roll), 0], [0, numpy.sin(roll), numpy.cos(roll), 0], [0, 0, 0, 1]]
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit a298fec02b8300e23841085a73c2039261c3d43c
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 8dadd23d3ee696ae781a8a81c5b34f2efa23b2ee
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit bd38e352ee32cabccc9aa0bc45d838f17941b541
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 593eedabbe35d33e9974b89370f76b212192d756
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit cfb48a74f4013c866231f05a29e3757dc517a18f
|
||||
Загрузка…
Ссылка в новой задаче