Merge branch 'master' of github.com:Microsoft/AI-Robot-Challenge
This commit is contained in:
Manuel Caballero Sanchez
Коммит
538b234fa9
|
|
@ -1,24 +0,0 @@
|
|||
# Robotics Demo
|
||||
|
||||
### How to set up the bot?
|
||||
|
||||
1. Pre-requisites:
|
||||
- [Python 3.6.4 or newer](https://www.python.org/downloads/)
|
||||
- [Bot Framework Emulator (V4 Preview)](https://github.com/Microsoft/BotFramework-Emulator/releases)
|
||||
1. Install the following python packages
|
||||
```
|
||||
pip3 install aiohttp
|
||||
pip3 install requests
|
||||
pip3 install botbuilder
|
||||
pip3 install botbuilder.schema
|
||||
pip3 install botbuilder.core
|
||||
```
|
||||
1. Run the bot
|
||||
- Open the `Command Prompt` from the **Start Menu**.
|
||||
- Move to the bot directory: `cd <your-bot-repo>/bot`
|
||||
- Execute: `py .\main.py`
|
||||
|
||||
1. Test your bot using the Bot Framework Emulator V4
|
||||
- Open the `Bot Framework Emulator` from the **Start Menu**.
|
||||
- Click **Open Bot** and select the bot configuration file: `<your-bot-repo>\bot\SawyerBot.bot`
|
||||
- Wait for the bot to load, you will receive a welcome message once is connected.
|
||||
|
|
@ -1 +1,21 @@
|
|||
Setup instructions
|
||||
# Ubuntu Setup Instructions
|
||||
|
||||
## Run the Install Script
|
||||
|
||||
1. Clone the repo.
|
||||
1. Navigate to `AI-Robot-Challenge/setup/ubuntu` 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`
|
||||
|
||||
## Run the Bot Locally
|
||||
|
||||
1. Navigate to `AI-Robot-Challenge/setup/ubuntu` in a Terminal window.
|
||||
1. Run the following command `python3.6 main.py`.
|
||||
|
||||
## Run the Emulator
|
||||
|
||||
1. Run `Bot Framework Emulator` from your applications.
|
||||
1. Click **Open Bot** and select the bot configuration file: `AI-Robot-Challenge/setup/ubuntu/SawyerBot.bot`
|
||||
1. You will be welcomed with the message `'Welcome to Sawyer Robot!'`.
|
||||
1. Send the message `move arm` or `show stats` to trigger intents.
|
||||
|
||||
|
|
|
|||
|
|
@ -15,12 +15,35 @@ sudo swapon -a
|
|||
# description="Microsoft Robot Challenge 2018"
|
||||
# version="1.0"
|
||||
|
||||
# Update to lateset software lists
|
||||
echo -e '***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***'
|
||||
#
|
||||
#
|
||||
#
|
||||
# Install Bot Framework + Emulator
|
||||
echo -e '***\n***\n***\n***\nInstall bot framework + Emulator\n***\n***\n***\n***'
|
||||
#
|
||||
#
|
||||
|
||||
# Install some common CLI tools
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y wget software-properties-common
|
||||
# Install Emulator
|
||||
sudo apt-get update
|
||||
sudo apt-get -f install libindicator7
|
||||
sudo apt-get -f install libappindicator1
|
||||
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_i386.deb'
|
||||
sudo dpkg -i "$TEMP_DEB"
|
||||
rm -f "$TEMP_DEB"
|
||||
|
||||
# Install Python 3.6
|
||||
sudo add-apt-repository ppa:deadsnakes/ppa
|
||||
sudo apt-get update
|
||||
sudo apt-get install python3.6
|
||||
sudo apt-get install python3-pip
|
||||
python3.6 -m pip install --upgrade pip
|
||||
|
||||
# Install Bot Framework Deps
|
||||
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
|
||||
|
||||
#
|
||||
#
|
||||
|
|
@ -31,6 +54,12 @@ echo -e '***\n***\n***\n***\nInstall ROS\n***\n***\n***\n***'
|
|||
#
|
||||
# http://sdk.rethinkrobotics.com/intera/Workstation_Setup
|
||||
|
||||
# Update to lateset software lists
|
||||
echo -e '***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***'
|
||||
|
||||
# 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'
|
||||
|
|
@ -221,4 +250,3 @@ echo -e '***\n***\n***\n***\nClean up all the temp files\n***\n***\n***\n***'
|
|||
sudo apt-get clean
|
||||
sudo rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
|
||||
|
||||
# TODO:// install VSCode
|
||||
|
|
@ -2,8 +2,16 @@ 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
|
||||
)
|
||||
|
||||
|
|
@ -22,7 +30,7 @@ catkin_package(
|
|||
#############
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/ik_pick_and_place_demo.py
|
||||
src/sorting_demo/program.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -7,6 +7,6 @@
|
|||
</include>
|
||||
|
||||
<!-- Start the Sawyer pick and place demo -->
|
||||
<node pkg="sorting_demo" type="sorting_demo.py" name="sorting_demo" />
|
||||
<node pkg="sorting_demo" type="program.py" name="sorting_demo" />
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,287 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import argparse
|
||||
import struct
|
||||
import sys
|
||||
import copy
|
||||
|
||||
import rospy
|
||||
import rospkg
|
||||
import xacro
|
||||
|
||||
from gazebo_msgs.srv import (
|
||||
SpawnModel,
|
||||
DeleteModel,
|
||||
)
|
||||
from geometry_msgs.msg import (
|
||||
PoseStamped,
|
||||
Pose,
|
||||
Point,
|
||||
Quaternion,
|
||||
)
|
||||
|
||||
import intera_interface
|
||||
|
||||
|
||||
class SortingDemo(object):
|
||||
def __init__(self, limb="right", hover_distance=0.15, tip_name="right_gripper_tip"):
|
||||
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()
|
||||
# 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):
|
||||
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 _guarded_move_to_joint_position(self, joint_angles, timeout=5.0):
|
||||
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 gripper_open(self):
|
||||
self._gripper.open()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def gripper_close(self):
|
||||
self._gripper.close()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def _approach(self, pose):
|
||||
approach = copy.deepcopy(pose)
|
||||
# approach with a pose the hover-distance above the requested pose
|
||||
approach.position.z = approach.position.z + self._hover_distance
|
||||
joint_angles = self._limb.ik_request(approach, self._tip_name)
|
||||
self._limb.set_joint_position_speed(0.001)
|
||||
self._guarded_move_to_joint_position(joint_angles)
|
||||
self._limb.set_joint_position_speed(0.1)
|
||||
|
||||
def _retract(self):
|
||||
# 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 + self._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(ik_pose)
|
||||
|
||||
def _servo_to_pose(self, 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 - pose.position.x) / steps
|
||||
ik_delta.position.y = (current_pose['position'].y - pose.position.y) / steps
|
||||
ik_delta.position.z = (current_pose['position'].z - pose.position.z) / steps
|
||||
ik_delta.orientation.x = (current_pose['orientation'].x - pose.orientation.x) / steps
|
||||
ik_delta.orientation.y = (current_pose['orientation'].y - pose.orientation.y) / steps
|
||||
ik_delta.orientation.z = (current_pose['orientation'].z - pose.orientation.z) / steps
|
||||
ik_delta.orientation.w = (current_pose['orientation'].w - 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 + pose.position.x
|
||||
ik_step.position.y = d * ik_delta.position.y + pose.position.y
|
||||
ik_step.position.z = d * ik_delta.position.z + pose.position.z
|
||||
ik_step.orientation.x = d * ik_delta.orientation.x + pose.orientation.x
|
||||
ik_step.orientation.y = d * ik_delta.orientation.y + pose.orientation.y
|
||||
ik_step.orientation.z = d * ik_delta.orientation.z + pose.orientation.z
|
||||
ik_step.orientation.w = d * ik_delta.orientation.w + 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()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def pick(self, pose):
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# open the gripper
|
||||
self.gripper_open()
|
||||
# servo above pose
|
||||
self._approach(pose)
|
||||
# servo to pose
|
||||
self._servo_to_pose(pose)
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# close gripper
|
||||
self.gripper_close()
|
||||
# retract to clear object
|
||||
self._retract()
|
||||
|
||||
def place(self, pose):
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# servo above pose
|
||||
self._approach(pose)
|
||||
# servo to pose
|
||||
self._servo_to_pose(pose)
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# open the gripper
|
||||
self.gripper_open()
|
||||
# retract to clear object
|
||||
self._retract()
|
||||
|
||||
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, 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, e:
|
||||
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
|
||||
|
||||
def load_gazebo_models():
|
||||
# Spawn Table
|
||||
table_name = "cafe_table"
|
||||
table_path = rospkg.RosPack().get_path('sawyer_sim_examples') + "/models/cafe_table/model.sdf"
|
||||
table_pose = Pose(position=Point(x=0.75, y=0.0, z=0.0))
|
||||
table_reference_frame = "world"
|
||||
spawn_sdf_model(table_name, table_path, table_pose, table_reference_frame)
|
||||
|
||||
# Spawn blocks
|
||||
block_path = rospkg.RosPack().get_path('sorting_demo') + "/models/block/block.urdf.xacro"
|
||||
block_reference_frame = "world"
|
||||
|
||||
block_poses = [
|
||||
Pose(position=Point(x=0.4225, y=0.1265, z=0.7725)),
|
||||
Pose(position=Point(x=0.60, y=0.1265, z=0.7725)),
|
||||
Pose(position=Point(x=0.4225, y=-0.1, z=0.7725))]
|
||||
block_mappings = [
|
||||
{"material" : "Gazebo/Green"},
|
||||
{"material" : "Gazebo/Orange"},
|
||||
{"material" : "Gazebo/SkyBlue"}]
|
||||
|
||||
for (i, pose, mappings) in zip(range(len(block_poses)), block_poses, block_mappings):
|
||||
spawn_xacro_model("block{}".format(i), block_path, pose, block_reference_frame, mappings)
|
||||
|
||||
def delete_gazebo_models():
|
||||
# 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)
|
||||
resp_delete = delete_model("cafe_table")
|
||||
resp_delete = delete_model("block")
|
||||
except rospy.ServiceException, e:
|
||||
print("Delete Model service call failed: {0}".format(e))
|
||||
|
||||
|
||||
def main():
|
||||
"""SDK Inverse Kinematics Pick and Place Example
|
||||
|
||||
A Pick and Place example using the Rethink Inverse Kinematics
|
||||
Service which returns the joint angles a requested Cartesian Pose.
|
||||
This ROS Service client is used to request both pick and place
|
||||
poses in the /base frame of the robot.
|
||||
|
||||
Note: This is a highly scripted and tuned demo. The object location
|
||||
is "known" and movement is done completely open loop. It is expected
|
||||
behavior that Sawyer will eventually mis-pick or drop the block. You
|
||||
can improve on this demo by adding perception and feedback to close
|
||||
the loop.
|
||||
"""
|
||||
rospy.init_node("ik_pick_and_place_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
|
||||
load_gazebo_models()
|
||||
# Remove models from the scene on shutdown
|
||||
rospy.on_shutdown(delete_gazebo_models)
|
||||
|
||||
limb = 'right'
|
||||
hover_distance = 0.15 # meters
|
||||
# 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}
|
||||
pnp = SortingDemo(limb, hover_distance)
|
||||
# An orientation for gripper fingers to be overhead and parallel to the obj
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167,
|
||||
y=0.999994209902,
|
||||
z=-0.00177030764765,
|
||||
w=0.00253311793936)
|
||||
block_poses = list()
|
||||
# The Pose of the block in its initial location.
|
||||
# You may wish to replace these poses with estimates
|
||||
# from a perception node.
|
||||
block_poses.append(Pose(
|
||||
position=Point(x=0.45, y=0.155, z=-0.129),
|
||||
orientation=overhead_orientation))
|
||||
# Feel free to add additional desired poses for the object.
|
||||
# Each additional pose will get its own pick and place.
|
||||
block_poses.append(Pose(
|
||||
position=Point(x=0.6, y=-0.1, z=-0.129),
|
||||
orientation=overhead_orientation))
|
||||
# Move to the desired starting angles
|
||||
print("Running. Ctrl-c to quit")
|
||||
pnp.move_to_start(starting_joint_angles)
|
||||
idx = 0
|
||||
while not rospy.is_shutdown():
|
||||
print("\nPicking...")
|
||||
pnp.pick(block_poses[idx])
|
||||
print("\nPlacing...")
|
||||
idx = (idx + 1) % len(block_poses)
|
||||
pnp.place(block_poses[idx])
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
import rospy
|
||||
import gazebo_msgs.msg._LinkStates
|
||||
from gazebo_msgs.msg import LinkStates
|
||||
|
||||
|
||||
class EnvironmentEstimation:
|
||||
|
|
@ -8,9 +8,9 @@ class EnvironmentEstimation:
|
|||
self.blocks = []
|
||||
|
||||
# initial simulated implementation
|
||||
pub = rospy.Subscriber('/gazebo/link_states', gazebo_msgs.msg._LinkStates, self.links_callback, queue_size=10)
|
||||
pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self._links_callback, queue_size=10)
|
||||
|
||||
def links_callback(self, links):
|
||||
def _links_callback(self, links):
|
||||
"""
|
||||
string[] name
|
||||
geometry_msgs/Pose[] pose
|
||||
|
|
@ -33,11 +33,29 @@ class EnvironmentEstimation:
|
|||
float64 y
|
||||
float64 z
|
||||
|
||||
|
||||
:param links:
|
||||
:return:
|
||||
"""
|
||||
pass
|
||||
blocks = []
|
||||
trays = []
|
||||
|
||||
for i, name in enumerate(links.name):
|
||||
if "block" in name:
|
||||
blocks.append(links.pose[i])
|
||||
elif "tray" in name:
|
||||
trays.append(links.pose[i])
|
||||
|
||||
self.blocks = blocks
|
||||
self.trays = trays
|
||||
|
||||
def get_blocks(self):
|
||||
"""
|
||||
:return array of geometry_msgs.msg.Pose
|
||||
"""
|
||||
return self.blocks
|
||||
|
||||
def get_trays(self):
|
||||
"""
|
||||
:return array of geometry_msgs.msg.Pose
|
||||
"""
|
||||
return self.trays
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
#!/usr/bin/env python
|
||||
import sys
|
||||
import functools
|
||||
|
||||
import rospkg
|
||||
import rospy
|
||||
import xacro
|
||||
|
||||
import geometry_msgs.msg
|
||||
from geometry_msgs.msg import Pose, Point,Quaternion
|
||||
from gazebo_msgs.srv import SpawnModel,DeleteModel
|
||||
|
||||
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, 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, e:
|
||||
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
|
||||
|
||||
def load_gazebo_models():
|
||||
model_list = []
|
||||
|
||||
# Spawn Table
|
||||
table_name = "cafe_table"
|
||||
table_path = rospkg.RosPack().get_path('sawyer_sim_examples') + "/models/cafe_table/model.sdf"
|
||||
table_pose = Pose(position=Point(x=0.75, y=0.0, z=0.0))
|
||||
table_reference_frame = "world"
|
||||
spawn_sdf_model(table_name, table_path, table_pose, table_reference_frame)
|
||||
model_list.append(table_name)
|
||||
|
||||
# Spawn blocks
|
||||
block_path = rospkg.RosPack().get_path('sorting_demo') + "/models/block/block.urdf.xacro"
|
||||
block_reference_frame = "world"
|
||||
|
||||
block_poses = [
|
||||
Pose(position=Point(x=0.4225, y=0.1265, z=0.7725)),
|
||||
Pose(position=Point(x=0.60, y=0.1265, z=0.7725)),
|
||||
Pose(position=Point(x=0.4225, y=-0.1, z=0.7725))]
|
||||
block_mappings = [
|
||||
{"material" : "Gazebo/Green"},
|
||||
{"material" : "Gazebo/Orange"},
|
||||
{"material" : "Gazebo/SkyBlue"}]
|
||||
|
||||
for (i, pose, mappings) in zip(range(len(block_poses)), block_poses, block_mappings):
|
||||
name = "block{}".format(i)
|
||||
spawn_xacro_model(name, block_path, pose, block_reference_frame, mappings)
|
||||
model_list.append(name)
|
||||
|
||||
return model_list
|
||||
|
||||
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, e:
|
||||
print("Delete Model service call failed: {0}".format(e))
|
||||
|
||||
|
||||
def main():
|
||||
"""SDK Inverse Kinematics Pick and Place Example
|
||||
|
||||
A Pick and Place example using the Rethink Inverse Kinematics
|
||||
Service which returns the joint angles a requested Cartesian Pose.
|
||||
This ROS Service client is used to request both pick and place
|
||||
poses in the /base frame of the robot.
|
||||
|
||||
Note: This is a highly scripted and tuned demo. The object location
|
||||
is "known" and movement is done completely open loop. It is expected
|
||||
behavior that Sawyer will eventually mis-pick or drop the block. You
|
||||
can improve on this demo by adding perception and feedback to close
|
||||
the loop.
|
||||
"""
|
||||
rospy.init_node("ik_pick_and_place_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 = load_gazebo_models()
|
||||
# Remove models from the scene on shutdown
|
||||
rospy.on_shutdown(functools.partial(delete_gazebo_models, model_list))
|
||||
|
||||
limb = 'right'
|
||||
hover_distance = 0.15 # meters
|
||||
# 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}
|
||||
|
||||
import sorting_robot
|
||||
from sorting_robot import SortingRobot
|
||||
pnp = SortingRobot(limb, hover_distance)
|
||||
# An orientation for gripper fingers to be overhead and parallel to the obj
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167,
|
||||
y=0.999994209902,
|
||||
z=-0.00177030764765,
|
||||
w=0.00253311793936)
|
||||
block_poses = list()
|
||||
# The Pose of the block in its initial location.
|
||||
# You may wish to replace these poses with estimates
|
||||
# from a perception node.
|
||||
block_poses.append(Pose(
|
||||
position=Point(x=0.45, y=0.155, z=-0.129),
|
||||
orientation=overhead_orientation))
|
||||
# Feel free to add additional desired poses for the object.
|
||||
# Each additional pose will get its own pick and place.
|
||||
block_poses.append(Pose(
|
||||
position=Point(x=0.6, y=-0.1, z=-0.129),
|
||||
orientation=overhead_orientation))
|
||||
# Move to the desired starting angles
|
||||
print("Running. Ctrl-c to quit")
|
||||
pnp.move_to_start(starting_joint_angles)
|
||||
idx = 0
|
||||
while not rospy.is_shutdown():
|
||||
print("\nPicking...")
|
||||
pnp.pick(block_poses[idx])
|
||||
print("\nPlacing...")
|
||||
idx = (idx + 1) % len(block_poses)
|
||||
pnp.place(block_poses[idx])
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
import copy
|
||||
|
||||
import rospy
|
||||
import rospkg
|
||||
|
||||
from geometry_msgs.msg import (
|
||||
Pose,
|
||||
)
|
||||
|
||||
import intera_interface
|
||||
|
||||
from object_detection.object_detection import EnvironmentEstimation
|
||||
|
||||
|
||||
class SortingRobot(object):
|
||||
def __init__(self, limb="right", hover_distance=0.15, tip_name="right_gripper_tip"):
|
||||
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()
|
||||
|
||||
#subcomponents
|
||||
self.environmentEstimation = EnvironmentEstimation()
|
||||
|
||||
# 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):
|
||||
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 _guarded_move_to_joint_position(self, joint_angles, timeout=5.0):
|
||||
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 gripper_open(self):
|
||||
self._gripper.open()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def gripper_close(self):
|
||||
self._gripper.close()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def _approach(self, pose):
|
||||
approach = copy.deepcopy(pose)
|
||||
# approach with a pose the hover-distance above the requested pose
|
||||
approach.position.z = approach.position.z + self._hover_distance
|
||||
joint_angles = self._limb.ik_request(approach, self._tip_name)
|
||||
self._limb.set_joint_position_speed(0.001)
|
||||
self._guarded_move_to_joint_position(joint_angles)
|
||||
self._limb.set_joint_position_speed(0.1)
|
||||
|
||||
def _retract(self):
|
||||
# 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 + self._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(ik_pose)
|
||||
|
||||
def _servo_to_pose(self, 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 - pose.position.x) / steps
|
||||
ik_delta.position.y = (current_pose['position'].y - pose.position.y) / steps
|
||||
ik_delta.position.z = (current_pose['position'].z - pose.position.z) / steps
|
||||
ik_delta.orientation.x = (current_pose['orientation'].x - pose.orientation.x) / steps
|
||||
ik_delta.orientation.y = (current_pose['orientation'].y - pose.orientation.y) / steps
|
||||
ik_delta.orientation.z = (current_pose['orientation'].z - pose.orientation.z) / steps
|
||||
ik_delta.orientation.w = (current_pose['orientation'].w - 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 + pose.position.x
|
||||
ik_step.position.y = d * ik_delta.position.y + pose.position.y
|
||||
ik_step.position.z = d * ik_delta.position.z + pose.position.z
|
||||
ik_step.orientation.x = d * ik_delta.orientation.x + pose.orientation.x
|
||||
ik_step.orientation.y = d * ik_delta.orientation.y + pose.orientation.y
|
||||
ik_step.orientation.z = d * ik_delta.orientation.z + pose.orientation.z
|
||||
ik_step.orientation.w = d * ik_delta.orientation.w + 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()
|
||||
rospy.sleep(1.0)
|
||||
|
||||
def pick(self, pose):
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# open the gripper
|
||||
self.gripper_open()
|
||||
# servo above pose
|
||||
self._approach(pose)
|
||||
# servo to pose
|
||||
self._servo_to_pose(pose)
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# close gripper
|
||||
self.gripper_close()
|
||||
# retract to clear object
|
||||
self._retract()
|
||||
|
||||
def place(self, pose):
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# servo above pose
|
||||
self._approach(pose)
|
||||
# servo to pose
|
||||
self._servo_to_pose(pose)
|
||||
if rospy.is_shutdown():
|
||||
return
|
||||
# open the gripper
|
||||
self.gripper_open()
|
||||
# retract to clear object
|
||||
self._retract()
|
||||
Загрузка…
Ссылка в новой задаче