class Place(object):
_feedback = moveit_msgs.msg.PlaceActionFeedback().feedback
_result = moveit_msgs.msg.PlaceActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PlaceAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Place action is ok. Awaiting for connections')
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
if len(self.scene.get_attached_objects()) < 1:
rospy.loginfo("No object attached")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self.robot.set_start_state_to_current_state()
self._feedback.state = "Planing to place pose"
self._as.publish_feedback(self._feedback)
target = [
goal.place_locations[0].place_pose.pose.position.x,
goal.place_locations[0].place_pose.pose.position.y,
goal.place_locations[0].place_pose.pose.position.z
]
quat = [
goal.place_locations[0].place_pose.pose.orientation.x,
goal.place_locations[0].place_pose.pose.orientation.y,
goal.place_locations[0].place_pose.pose.orientation.z,
goal.place_locations[0].place_pose.pose.orientation.w
]
if np.sum(quat) == 0:
# not valid quaternion
quat = []
rospy.loginfo('Place quaternion [%s]', quat)
plan = self.robot.ef_pose(list(target), orientation=quat)
if plan is None:
rospy.loginfo("Plan to place failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._feedback.state = "Going to place the object"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to place the object")
self._result.trajectory_stages.append(plan)
ex_status = self.robot.arm_execute(plan)
if not ex_status:
rospy.loginfo("Execution to place failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
rospy.sleep(1)
self._feedback.state = "Planning to open the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Openning gripper"
print self._feedback
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to open gripper failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
rospy.sleep(1)
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
self._as.set_succeeded(self._result)
class CollisionEnv(object):
NS = "/art/collision_env/"
def __init__(self, setup, world_frame):
assert setup != ""
self.ready = False
self.setup = setup
self.world_frame = world_frame
self.tf_listener = TransformListener()
self.api = ArtApiHelper()
rospy.loginfo("Waiting for DB API")
self.api.wait_for_db_api()
self.ignored_prefixes = array_from_param("~ignored_prefixes")
rospy.loginfo("Will ignore following prefixes: " +
str(self.ignored_prefixes))
self.lock = RLock()
self.ps = PlanningSceneInterface()
self._paused = False
self.oh = ObjectHelper(self.object_cb)
self.artificial_objects = {}
self.collision_objects_pub = rospy.Publisher(self.NS + "artificial",
CollisionObjects,
latch=True,
queue_size=1)
self.pub_artificial()
self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)
self.paused_pub = rospy.Publisher(self.NS + "paused",
Bool,
latch=True,
queue_size=1)
self.paused = False
def start(self):
self.ready = True
rospy.loginfo("Ready")
def load_from_db(self):
for prim in self.api.get_collision_primitives(self.setup):
rospy.loginfo("Loading object: " + prim.name)
self.add_primitive(prim)
def save_primitive(self, name):
with self.lock:
if name not in self.artificial_objects:
raise CollisionEnvException("Unknown object name")
p = self.artificial_objects[name]
self.api.add_collision_primitive(p)
def save_primitives(self):
with self.lock:
for name in self.artificial_objects.keys():
self.save_primitive(name)
def set_primitive_pose(self, name, ps):
with self.lock:
p = self.artificial_objects[name]
assert p.pose.header.frame_id == ps.header.frame_id
p.pose.pose = ps.pose
self.ps.add_box(p.name, p.pose, p.bbox.dimensions)
self.pub_artificial()
def add_primitive(self, p):
with self.lock:
self.artificial_objects[p.name] = p
self.ps.add_box(p.name, p.pose, p.bbox.dimensions)
self.pub_artificial()
def pub_artificial(self):
msg = CollisionObjects()
for v in self.artificial_objects.values():
# transform all collision primitives into world frame (= marker)
if v.pose.header.frame_id != self.world_frame:
try:
self.tf_listener.waitForTransform(v.pose.header.frame_id,
self.world_frame,
rospy.Time(0),
rospy.Duration(1.0))
v.pose = self.tf_listener.transformPose(
self.world_frame, v.pose)
except tf.Exception as e:
rospy.logwarn("Failed to transform artificial object: " +
str(e))
continue
msg.primitives.append(v)
self.collision_objects_pub.publish(msg)
@property
def paused(self):
return self._paused
@paused.setter
def paused(self, val):
self._paused = val
self.paused_pub.publish(val)
def remove_name(self, name):
with self.lock:
if name not in self.artificial_objects:
rospy.logwarn("Unknown artificial object: " + name)
return False
self.ps.remove_world_object(name)
del self.artificial_objects[name]
self.pub_artificial()
if not self.api.clear_collision_primitives(self.setup, names=[name]):
rospy.logwarn("Failed to remove from permanent storage")
rospy.loginfo("Removed object: " + name)
return True
def clear_all(self, permanent=True):
with self.lock:
rospy.loginfo("Clearing " + str(len(self.artificial_objects)) +
" artificial objects...")
for k in self.artificial_objects.keys():
if self.is_ignored(k):
continue
self.ps.remove_world_object(k)
self.artificial_objects = {}
self.pub_artificial()
try:
if permanent and not self.api.clear_collision_primitives(
self.setup):
rospy.logwarn("Failed to remove from permanent storage")
except ArtApiException as e:
rospy.logerr(str(e))
def reload(self):
self.clear_all(permanent=False)
self.load_from_db()
self.pub_artificial()
def _generate_name(self):
# as we use only part of uuid, there might be collisions...
while True:
name = str(uuid.uuid4())[:8]
if name not in self.artificial_objects.keys():
break
return name
def set_det_pose(self, name, ps):
object_type = self.api.get_object_type(
self.oh.objects[name].object_type)
if object_type is not None:
self.add_detected(name, ps, object_type)
def clear_det_on_table(self, inv=False, ignore=None):
if ignore is None:
ignore = []
ret = []
with self.lock:
for v in self.oh.objects.values():
if v.object_id in ignore:
continue
if not inv and not v.on_table:
continue
if inv and v.on_table:
continue
ret.append(v.object_id)
self.clear_detected(v.object_id)
return ret
def is_ignored(self, name):
for ip in self.ignored_prefixes:
if name.startswith(ip):
return True
return False
def timer_cb(self, evt):
if self.paused:
return
with self.lock:
known_objects = self.ps.get_known_object_names()
for name in known_objects:
if name not in self.artificial_objects and name not in self.oh.objects and not self.is_ignored(
name):
rospy.loginfo("Removing outdated object: " + name)
self.clear_detected(name)
# restore artificial objects if they are lost somehow (e.g. by restart of MoveIt!)
for k, v in self.artificial_objects.iteritems():
if k in known_objects:
continue
rospy.loginfo("Restoring artificial object: " + v.name)
if v.bbox.type == SolidPrimitive.BOX:
self.ps.add_box(v.name, v.pose, v.bbox.dimensions)
else:
# TODO other types
pass
def object_cb(self, evt, h, inst):
if self.paused or not self.ready:
return
with self.lock:
attached_objects = self.ps.get_attached_objects()
if evt in (ObjectHelper.OBJECT_ADDED, ObjectHelper.OBJECT_UPDATED):
if inst.object_id in attached_objects:
return
ps = PoseStamped()
ps.header = h
ps.pose = inst.pose
object_type = self.api.get_object_type(inst.object_type)
if object_type is not None:
self.add_detected(inst.object_id, ps, object_type)
elif evt == ObjectHelper.OBJECT_LOST:
if inst.object_id not in attached_objects:
self.clear_detected(inst.object_id)
def add_detected(self, name, ps, object_type):
with self.lock:
self.ps.add_box(name, ps, object_type.bbox.dimensions)
def clear_detected(self, name):
with self.lock:
self.ps.remove_world_object(name)
def clear_all_det(self, ignore=None):
if ignore is None:
ignore = []
ret = []
with self.lock:
for v in self.oh.objects.values():
name = v.object_id
if name in ignore:
continue
self.clear_detected(name)
ret.append(name)
rospy.loginfo("Removed " + str(len(ret)) + " detected objects.")
return ret
def get_attached(self, transform_to_world=True):
"""
keep in mind - attached objects might not be detected
"""
ret = []
with self.lock:
ao = self.ps.get_attached_objects()
for k, v in ao.iteritems():
if len(v.object.primitives) != 1 or len(
v.object.primitive_poses) != 1:
rospy.logwarn("Unsupported type of attached object: " + k)
continue
if v.object.primitives[0].type != SolidPrimitive.BOX:
rospy.logwarn(
"Only box-like attached objects are supported so far.")
continue
ps = PoseStamped()
ps.header = v.object.header
ps.pose = v.object.primitive_poses[0]
if transform_to_world and ps.header.frame_id != self.world_frame:
try:
self.tf_listener.waitForTransform(ps.header.frame_id,
self.world_frame,
ps.header.stamp,
rospy.Duration(1.0))
ps = self.tf_listener.transformPose(self.world_frame, ps)
except tf.Exception as e:
rospy.logwarn("Failed to transform attached object: " +
str(e))
continue
ret.append((k, ps, v.object.primitives[0]))
return ret
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " +
str(err))
raise err
self.gripper_opened = [
rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01
]
self.gripper_closed = [
rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01
]
self.gripper_neutral = [
rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0]) /
2.0)
]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(0.01)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
rospy.loginfo("Scaling for MoveIt timeout=" + str(
rospy.get_param(
'/move_group/trajectory_execution/allowed_execution_duration_scaling'
)))
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
self.scene.remove_world_object(table_id)
self.scene.remove_world_object(box1_id)
self.scene.remove_world_object(box2_id)
self.scene.remove_world_object(target_id)
self.scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
# Set the height of the table off the ground
table_ground = 0.4
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.005, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.36
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
box1_pose.pose.position.y = 0.0
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
self.scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
box2_pose.pose.position.y = 0.2
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
self.scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id],
[target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(target_id, place_pose)
success = False
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.header.stamp = rospy.get_rostime()
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(0.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = GRIPPER_FRAME
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self,
initial_pose_stamped,
allowed_touch_objects,
grasp_opening=[0]):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.1, 0.15, [0.0, -1.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4]
# Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
# from arm base to the object to pick (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped,
ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1, -0.1]]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for yaw in yaw_vals:
for pitch in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, target_id, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]
# A list of pitch angles to try
pitch_vals = [0] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for pitch in pitch_vals:
for dy in y_vals:
for dx in x_vals:
place.pose.position.x = init_pose.pose.position.x + dx
place.pose.position.y = init_pose.pose.position.y + dy
# Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
# arm base to the place location (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(
place, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw = atan2(y, x)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
# the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
# objects orientation!), so we cancel this transformation. It is applied here:
# https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
# More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
acobjs = self.scene.get_attached_objects([target_id])
aco_pose = self.get_pose(acobjs[target_id])
if aco_pose is None:
rospy.logerr(
"Attached collision object '%s' not found" %
target_id)
return None
aco_tf = self.pose_to_mat(aco_pose)
place_tf = self.pose_to_mat(place.pose)
place.pose = self.mat_to_pose(place_tf, aco_tf)
rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
% (aco_pose, place.pose))
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
# Set the color of an object
def setColor(self, name, r, g, b, a=0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def get_pose(self, co):
# We get object's pose from the mesh/primitive poses; try first with the meshes
if isinstance(co, CollisionObject):
if co.mesh_poses:
return co.mesh_poses[0]
elif co.primitive_poses:
return co.primitive_poses[0]
else:
rospy.logerr(
"Collision object '%s' has no mesh/primitive poses" %
co.id)
return None
elif isinstance(co, AttachedCollisionObject):
if co.object.mesh_poses:
return co.object.mesh_poses[0]
elif co.object.primitive_poses:
return co.object.primitive_poses[0]
else:
rospy.logerr(
"Attached collision object '%s' has no mesh/primitive poses"
% co.id)
return None
else:
rospy.logerr("Input parameter is not a collision object")
return None
def pose_to_mat(self, pose):
'''Convert a pose message to a 4x4 numpy matrix.
Args:
pose (geometry_msgs.msg.Pose): Pose rospy message class.
Returns:
mat (numpy.matrix): 4x4 numpy matrix
'''
quat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
mat = np.matrix(quaternion_matrix(quat))
mat[0:3, 3] = pos
return mat
def mat_to_pose(self, mat, transform=None):
'''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.
Args:
mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
transform (numpy.ndarray): Optional 4x4 array representing additional transform
Returns:
pose (geometry_msgs.msg.Pose): Pose message representing transform.
'''
if transform != None:
mat = np.dot(mat, transform)
pose = Pose()
pose.position.x = mat[0, 3]
pose.position.y = mat[1, 3]
pose.position.z = mat[2, 3]
quat = quaternion_from_matrix(mat)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
class PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
class Place(object):
_feedback = moveit_msgs.msg.PlaceActionFeedback().feedback
_result = moveit_msgs.msg.PlaceActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PlaceAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Place action is ok. Awaiting for connections')
def get_target(self):
target = [0.17, 0.10, 0.028]
return target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
if len(self.scene.get_attached_objects()) < 1:
rospy.loginfo("No object attached")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self.robot.set_start_state_to_current_state()
self._feedback.state = "Planing to place pose"
self._as.publish_feedback(self._feedback)
target = [
goal.place_locations[0].place_pose.pose.position.x,
goal.place_locations[0].place_pose.pose.position.y,
goal.place_locations[0].place_pose.pose.position.z
]
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to place failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Going to place the object"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to place the object")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
self._feedback.state = "Removing object to be placed from the planning scene"
self._as.publish_feedback(self._feedback)
obj = self.scene.get_attached_objects()
link = obj[obj.keys()[0]].link_name
obj = obj[obj.keys()[0]].object
self.scene.remove_attached_object(link, obj.id)
self._feedback.state = "Planning to open the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Openning gripper"
print self._feedback
self.robot.gripper_execute(plan)
rospy.sleep(1)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Re-adding object"
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.add_box(obj.id, pose, obj.primitives[0].dimensions)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Planing to retreat after place"
self._as.publish_feedback(self._feedback)
target[2] += 0.02
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to retreat failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Retreating"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append("Retreat")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
if sucess:
self._result.error_code.val = 1
rospy.loginfo(self._result)
self._as.set_succeeded(self._result)
class Scene:
interface = None # type: PlanningSceneInterface
publisher = None # type: rospy.Publisher
ref_frame = '' # type: str
colors = dict() # type: dict
default_color = (0.75, 0.75, 0.75, 1) # type: tuple
def __init__(self, ref_frame='base_link'):
self.interface = PlanningSceneInterface()
self.publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
self.ref_frame = ref_frame
rospy.sleep(1)
def destroy(self):
pass
# ---- add & remove ---- #
def add_box(self, name, size, transform, rgba=None):
if rgba is None:
rgba = self.default_color
self.interface.remove_world_object(name)
pose = PoseStamped()
pose.header.frame_id = self.ref_frame
pose.pose = frame_to_pose(transform)
self.interface.add_box(name, pose, size)
self.wait_state_update(name)
self.set_color(name, rgba)
self.send_colors()
def remove_object(self, name):
self.interface.remove_world_object(name)
self.wait_state_update(name, is_known=False)
# ---- utils ---- #
def set_color(self, name, rgba):
color = ObjectColor()
color.id = name
color.color.r = rgba[0]
color.color.g = rgba[1]
color.color.b = rgba[2]
color.color.a = rgba[3]
self.colors[name] = color
def send_colors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.publisher.publish(p)
def wait_state_update(self, name, is_known=True, is_attached=False, timeout=4):
start = rospy.get_time()
current = rospy.get_time()
while (current - start < timeout) and not rospy.is_shutdown():
attached_objects = self.interface.get_attached_objects([name])
cur_is_attached = len(attached_objects.keys()) > 0
cur_is_known = name in self.interface.get_known_object_names()
if (cur_is_attached == is_attached) and (cur_is_known == is_known):
rospy.loginfo('Scene update succeeded.')
return True
rospy.sleep(0.1)
current = rospy.get_time()
# rospy.loginfo("Wait scene update [" + str(current - start) + "s]...")
rospy.loginfo('Scene update failed')
return False