Commit 942e4570 authored by Mathieu Reymond's avatar Mathieu Reymond
Browse files

Merge branch 'planning'

Conflicts:
	src/test/test.py
parents ddf702a0 1f498fad
<robot name="block">
<link name="block">
<inertial>
<origin xyz="0.025 0.025 0.025" />
<mass value="0.5" />
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5" />
</inertial>
<visual>
<origin xyz="0.025 0.025 0.025"/>
<geometry>
<box size="0.05 0.05 0.05" />
</geometry>
</visual>
<collision>
<origin xyz="0.025 0.025 0.025"/>
<geometry>
<box size="0.05 0.05 0.05" />
</geometry>
</collision>
</link>
<gazebo reference="block">
<material>Gazebo/Red</material>
<mu1>1000</mu1>
<mu2>1000</mu2>
</gazebo>
</robot>
<?xml version="1.0" ?>
<gazebo version="1.2">
<model name="cube_20k">
<link name="link">
<pose>0 0 0.5 0 0 0</pose>
<collision name="collision">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</visual>
</link>
</model>
</gazebo>
<?xml version="1.0" ?>
<sdf version="1.3">
<model name="cube_20k">
<link name="link">
<pose>0 0 0.5 0 0 0</pose>
<collision name="collision">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="cube_20k">
<link name="link">
<pose>0 0 0.02 0 0 0</pose>
<collision name="collision">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.02 0.02 0.02</scale>
</mesh>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.02 0.02 0.02</scale>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>Cube 20k</name>
<version>1.0</version>
<sdf version="1.2">model-1_2.sdf</sdf>
<sdf version="1.3">model-1_3.sdf</sdf>
<sdf version="1.4">model-1_4.sdf</sdf>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Mihai Dhola</name>
<email>mihai@</email>
</author>
<description>
A cube composed of 20,000 triangles. This is used for testing.
</description>
</model>
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="cube_20k">
<link name="link">
<pose>0 0 0.5 0 0 0</pose>
<collision name="collision">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://cube_20k/meshes/cube_20k.stl</uri>
<scale>0.5 0.5 0.5</scale>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>
......@@ -3,7 +3,7 @@ from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup()
d['packages'] = ['test']
d['packages'] = ['planning']
d['package_dir'] = {'': 'src'}
setup(**d)
import sys
import time
import rospy
import baxter_interface
from baxter_pykdl import baxter_kinematics
import moveit_commander
from baxter_interface import Gripper
import moveit_msgs.msg
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from sensor_msgs.msg import Image
import cv_bridge as cvb
import cv2
class Arm(moveit_commander.MoveGroupCommander):
def __init__(self, name, neutral=None, top=None, pick=None, grasp=None):
super(Arm, self).__init__(name)
self.neutral = neutral
self.pick = pick
self.top = top
self.grasp = grasp
name = self.get_name()[:-4] #remove _arm from name)
self.bridge = cvb.CvBridge()
self.kin = baxter_kinematics(name)
self.gripper = Gripper(name)
def current_pose(self):
return self.kin.forward_position_kinematics()
def move(self, pose):
self.set_pose_target(pose)
self.go()
def _msg_to_jpg(self, msg, name):
try:
# Convert your ROS Image message to OpenCV2
cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except cvb.CvBridgeError, e:
print(e)
else:
# Save your OpenCV2 image as a jpeg
cv2.imwrite(name + '.jpg', cv2_img)
def _regular_snapshot(self, msg, name='snap', timeout=1):
self._msg_to_jpg(msg, name)
time.sleep(timeout)
def ready_camera(self):
arm = self.get_name()[:-4] #remove _arm from name
callback = lambda m: self._regular_snapshot(m)
subscriber = rospy.Subscriber('cameras/'+arm+'_hand_camera/image', Image, callback)
baxter_interface.CameraController(arm+'_hand_camera').open()
class Arms(object):
def __init__(self, name, *args):
self._all = moveit_commander.MoveGroupCommander(name)
self._arms = args
self.neutral = [arm.neutral for arm in self._arms]
def current_pose(self):
return self._all.get_current_pose()
def set_pose_target(self, *args):
if len(args) is 1:
args = args[0]
for i, pose in enumerate(args):
self._all.set_pose_target(pose, end_effector_link=self._arms[i].get_end_effector_link())
def clear_pose_target(self):
for arm in self._arms:
self._all.clear_pose_target(arm.get_end_effector_link())
def plan(self):
self._all.plan()
def go(self, wait=True):
self._all.go(wait=wait)
def move(self, *args):
if len(args) is 1:
args = args[0]
self.set_pose_target(*args)
self.go()
class Baxter(object):
def __init__(self):
self.left = Arm('left_arm', neutral=[0.64623441, 0.84175585, 0.0570087, -0.3827256, 0.92221457, 0.02060252, 0.05115608])
self.right = Arm('right_arm',
neutral=[0.649446, -0.83735957, 0.06168316, 0.38299498, 0.92211736, -0.02071508, 0.050846],
top=[0.74667248, 0.02808032, 0.50554368, -0.00295978, 0.97005472, -0.02409497, 0.24167026],
pick=[7.54538492e-01, 3.44879303e-04, -6.50381151e-02, 5.63301315e-03, 9.99927904e-01, -9.66346662e-03, -4.36723168e-03],
grasp=[0.75390833, -0.00113117, -0.14, 0.00568284, 0.99992606, -0.00980662, -0.0044056])
self.both = Arms('both_arms', self.left, self.right)
self._commander = moveit_commander.RobotCommander()
baxter_interface.RobotEnable().enable()
rospy.sleep(1)
def _arm_by_name(self, name):
return self.arms[self.arm_names.index(name)]
def to_pose(self, *args):
pos = ['x', 'y', 'z']
ori = ['x', 'y', 'z', 'w']
if len(args) is 1:
args = args[0]
p = Point(**dict(zip(pos, args[:3])))
q = Quaternion(**dict(zip(ori, args[-4:])))
return Pose(position=p, orientation=q)
def to_list(self, pose):
pos = ['x', 'y', 'z']
ori = ['x', 'y', 'z', 'w']
return [getattr(pose.position, n) for n in pos], [getattr(pose.orientation, n) for n in ori]
def get_planning_frame(self):
return self._commander.get_planning_frame()
#!/usr/bin/python
import moveit_commander
import rospy
import sys
from baxter import Baxter
from scene import Scene
from geometry_msgs.msg import Pose, Point
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('example', anonymous=True)
b = Baxter()
s = Scene(b.get_planning_frame())
rospy.sleep(2)
s.clear_scene()
s.set_table()
s.set_item()
rospy.sleep(2)
# initial
b.both.move(b.both.neutral)
b.right.gripper.close()
rospy.sleep(3)
# approach
b.right.move(b.right.pick)
b.right.gripper.open()
rospy.sleep(3)
b.right.move(b.right.grasp)
rospy.sleep(1)
b.right.gripper.close()
rospy.sleep(1)
b.right.move(b.right.pick)
# go away and drop
b.right.move(b.right.neutral)
b.right.gripper.open()
import rospkg
import rospy
from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped
from gazebo_msgs.srv import SpawnModel, DeleteModel
class Scene(PlanningSceneInterface):
table_height = 0.8
item_height = 0.05
floor=-0.55
def __init__(self, frame):
super(Scene, self).__init__()
self.frame = frame
def clear_scene(self):
for name in self.get_known_object_names():
self.remove_world_object(name)
def set_table(self):
pose = PoseStamped()
pose.header.frame_id = self.frame
pose.pose.position.x = 1.0
pose.pose.position.y = 0.0
pose.pose.position.z = self.floor
self.add_box('cafe_table', pose, (1., 1., self.table_height))
pose.pose.position.z = 0.0 # gazebo reference is not the same as moveit
self.spawn('cafe_table', pose.pose)
def set_item(self):
pose = PoseStamped()
pose.header.frame_id = self.frame
pose.pose.position.x = 0.75
pose.pose.position.y = 0.0
pose.pose.position.z = self.floor + self.table_height/2 + self.item_height/2
self.add_box('block', pose, (0.04, 0.04, self.item_height))
pose.pose.position.x = 0.735
pose.pose.position.y = -0.025
pose.pose.position.z = self.table_height # gazebo reference is not the same as moveit
self.spawn('block', pose.pose, type='urdf')
def spawn(self, model, pose, model_name=None, type='sdf'):
if model_name is None:
model_name = model
# get model path
model_path = rospkg.RosPack().get_path('garbage_bot') + '/models/'
# Load Model SDF
xml = ''
with open (model_path + model + '/model-1_4.' + type, 'r') as f:
xml=f.read().replace('\n', '')
if type is 'sdf':
# 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(model_name, xml, '/',
pose, self.frame[1:])
except rospy.ServiceException, e:
rospy.logerr('Spawn SDF service call failed: {0}'.format(e))
else:
# Spawn Block URDF
rospy.wait_for_service('/gazebo/spawn_urdf_model')
try:
spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
resp_urdf = spawn_urdf(model_name, xml, "/",
pose, self.frame[1:])
except rospy.ServiceException, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.3, 0.1, 1.0))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
scene.add_box("table", p, (0.5, 1.5, 0.35))
p.pose.position.x = 0.6
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("part", p, (0.15, 0.1, 0.3))
rospy.sleep(1)
# pick an object
robot.right_arm.pick("part")
rospy.spin()
roscpp_shutdown()
#!/usr/bin/env python
import rospy
from moveit_python import MoveGroupInterface, PlanningSceneInterface
rospy.init_node('moveit_py')
g = MoveGroupInterface('left_arm', '/world')
# create a planning scene interface, provide name of root link
p = PlanningSceneInterface('world')
# add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame
p.addCube('my_cube', 0.1, 1, 0, 0.5)
joints = ['left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1']
positions = [0.6699952259595108,
1.030009435085784,
-0.4999997247485215,
-1.189968899785275,
1.9400238130755056,
-0.08000397926829805,
-0.9999781166910306]
g.moveToJointPosition(joints, positions)
#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
from geometry_msgs.msg import PoseStamped, Pose
table_height = 0.7
item_height = 0.1
floor=-0.55
def clear_scene(scene):
for name in scene.get_known_object_names():
scene.remove_world_object(name)
def set_table(scene, frame):
pose = PoseStamped()
pose.header.frame_id = frame
pose.pose.position.x = 1.0
pose.pose.position.y = 0.0
pose.pose.position.z = floor
scene.add_box('table', pose, (1., 1., table_height))
def set_item(scene, frame):
pose = PoseStamped()
pose.header.frame_id = frame
pose.pose.position.x = 0.75
pose.pose.position.y = 0.0
pose.pose.position.z = floor + table_height/2 + item_height/2
scene.add_box('item', pose, (0.1, 0.1, item_height))
def get_rest_pose():
pose = Pose()
pose.position.x = 0.05
pose.position.y = 1.0
pose.position.z = floor + 0.25
pose.orientation.x = 0.4
pose.orientation.y = 0.9
pose.orientation.z = 0.
pose.orientation.w = 0.
return pose
def get_item_pose(scene, item_name, frame):
pose = PoseStamped()
pose.header.frame_id = frame
poses = scene.get_object_poses([item_name])
pose.pose = poses[item_name]
return pose
def get_item_grasp(scene, item_name, frame):
g = Grasp()
g.grasp_pose = get_item_pose(scene, item_name, frame)
# define the pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = frame
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 0.0
g.pre_grasp_approach.min_distance = 0.05
g.pre_grasp_approach.desired_distance = 0.1
g.pre_grasp_posture.header.frame_id = 'left_gripper'
g.pre_grasp_posture.joint_names = ['l_gripper_l_finger']
pos = JointTrajectoryPoint()
pos.positions.append(0.0)
g.pre_grasp_posture.points.append(pos)
# set the grasp posture
g.grasp_posture.header.frame_id = 'left_gripper'
g.grasp_posture.joint_names = ['l_gripper_l_finger']
pos = JointTrajectoryPoint()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = frame
g.post_grasp_retreat.direction.vector.x = 0.0
g.post_grasp_retreat.direction.vector.y = 0.0
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.desired_distance = 0.25
g.post_grasp_retreat.min_distance = 0.05
grasps.append(g)
moveit_commander.roscpp_initialize(sys.argv)