def create_move_group_pose_goal(goal_pose=Pose(),
group="right_arm_torso",
end_link_name="arm_right_tool_link",
plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose"""
# Specifying the header makes the planning fail...
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 1
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def add_box_obstacle(self, dimensions, name, com_position, com_orientation):
"""
Add a rectangular prism obstacle to the planning scene.
Arguments:
dimensions: An array containing the width, length, and height of
the box (in the box's body frame, corresponding to x, y, and z).
name: A unique name for identifying this obstacle.
com_position: The position of the center-of-mass (COM) of this box,
relative to the global frame `frame_id`.
com_orientation: The orientation of the COM.
"""
pose = create_pose(self.frame_id, com_position, com_orientation)
obj = CollisionObject()
obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header
box = SolidPrimitive()
box.type, box.dimensions = SolidPrimitive.BOX, dimensions
obj.primitives, obj.primitive_poses = [box], [pose.pose]
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Added box object "{}" to planning scene: '
'(x={}, y={}, z={}).'.format(name, *dimensions))
def test_add_collision_object(self):
planning_scene_diff = PlanningScene(is_diff=True)
rospy.sleep(1)
pose_stamped_block = Pose()
# pose_stamped_block.header.frame_id = self.moveit_frame
pos = Point(0.6, 0., 0.03)
pos.z += 0.03
pose_stamped_block.position = pos
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.045, 0.045, 0.06]
test_block = CollisionObject()
test_block.operation = CollisionObject.ADD
test_block.primitive_poses.append(pose_stamped_block)
test_block.primitives.append(primitive)
planning_scene_diff.world.collision_objects.append(test_block)
self.planning_scene_pub.publish(planning_scene_diff)
rospy.sleep(2)
def create_move_group_pose_goal(self,
goal_pose=Pose(),
group="manipulator",
end_link_name=None,
plan_only=True):
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None:
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.1]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 0.01
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def addBox(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
use_service=True,
frame_id=None):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, use_service)
def __add_virtual_obstacle(name, x, y, operation=CollisionObject.ADD):
sp = SolidPrimitive()
sp.type = SolidPrimitive.BOX
sp.dimensions = [0.5, 0.5, 0.1]
spp = PoseStamped()
spp.header.frame_id = 'map'
spp.pose.position.x = x
spp.pose.position.y = y
spp.pose.position.z = 0.05
spp.pose.orientation.w = 1
co = CollisionObject()
co.operation = operation
co.id = name
co.type.db = json.dumps({'table': 'NONE', 'type': 'obstacle', 'name': co.id})
co.header.frame_id = 'map'
co.header.stamp = rospy.get_rostime()
co.primitives.append(sp)
co.primitive_poses.append(TF2().transform_pose(spp, spp.header.frame_id, co.header.frame_id).pose)
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Added a fake obstacle named '%s'", name)
def addCone(self, name, height, radius, pos_x, pos_y, pos_z):
group = self.group
scene = self.scene
eef_link = self.eef_link
robot = self.robot
s = SolidPrimitive()
s.dimensions = [height, radius]
s.type = s.CONE
ps = PoseStamped()
ps.header.frame_id = self.planning_frame
ps.pose.position.x = pos_x
ps.pose.position.y = pos_y
ps.pose.position.z = pos_z
ps.pose.orientation.w = 1.0
ps.pose.orientation.x = 2.0
ps.pose.orientation.y = 3.0
ps.pose.orientation.z = 2.0
scene.add_cone(name, ps, height, radius)
def apple_pose_callback(self, message):
if self.pick_place_finished or self.first_time_grasp:
self.first_time_grasp = False
self.pick_place_finished = False
print("apple_pose_callback")
apple = SolidPrimitive()
apple.type = SolidPrimitive.SPHERE
apple.dimensions = [0.03, 0.03, 0.03]
self.object.name = "apple"
self.object.primitives = [apple]
self.object.primitive_poses = [message]
# add stamp and frame
self.object.header.stamp = rospy.Time.now()
self.object.header.frame_id = "base_link"
goal = GraspPlanningGoal()
goal.object = self.object
self.grasp_planner_client.send_goal(goal)
self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
grasp_planner_result = self.grasp_planner_client.get_result()
self.grasps = grasp_planner_result.grasps
self.ready_for_grasp = True
def add_board_object():
# Some publisher
scene_diff_publisher = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1)
rospy.sleep(5.0)
# Create board object
board = CollisionObject()
board.header.frame_id = 'base'
board.id = 'board'
board_box = SolidPrimitive()
board_box.type = 1
# board_box.dimensions = [3.0, 4.0, 0.185]
board_box.dimensions = [DEPTH_BIAS * 2, 4.0, 3.0]
board.primitives.append(board_box)
board_pose = Pose()
board_pose.position.x = transformed_message.pose.position.x
board_pose.position.y = transformed_message.pose.position.y
board_pose.position.z = transformed_message.pose.position.z
# board_pose.orientation.x = transformed_message.pose.orientation.x
board_pose.orientation.x = 0
# board_pose.orientation.y = transformed_message.pose.orientation.y
board_pose.orientation.y = 0
# board_pose.orientation.z = transformed_message.pose.orientation.z
board_pose.orientation.z = 0
# board_pose.orientation.w = transformed_message.pose.orientation.w
board_pose.orientation.w = 0
board.primitive_poses.append(board_pose)
scene = PlanningScene()
scene.world.collision_objects.append(board)
scene.is_diff = True
scene_diff_publisher.publish(scene)
def create_collision_object(shape_type, pos, size, frame_id, op, object_id):
col = CollisionObject()
col.id = object_id
col.operation = op
col.header.frame_id = frame_id
# create primitive
primitive = SolidPrimitive()
primitive.type = shape_type
primitive.dimensions = size
# create pose
pose = Pose()
pose.position.x = pos[0]
pose.position.y = pos[1]
pose.position.z = pos[2]
pose.orientation.x = pose.orientation.y = pose.orientation.z = 0
pose.orientation.w = 1
col.primitives = [primitive]
col.primitive_poses = [pose]
return col
def roof():
'''
Create a roof that constrains the random movements of the robot.
@return An AttachedCollisionObject representing the roof
'''
roof = AttachedCollisionObject()
roof.link_name = "base_link"
roof.object.header.frame_id = "world"
roof.object.id = "roof"
roofPose = Pose()
roofPose.position.z = .6
roofPose.orientation.w = 1.0
roofBox = SolidPrimitive()
roofBox.type = roofBox.BOX
roofBox.dimensions = [1.5, 1, 0.025]
roof.object.primitives.append(roofBox)
roof.object.primitive_poses.append(roofPose)
return roof
def add_arbitrary_obstacle(size, id, pose, planning_scene_publisher, scene,
robot, operation):
"""
Adds an arbitrary rectangular prism obstacle to the planning scene.
This is currently only for the ground plane
size: numpy array of size 3 (x,y,z dimensions)
id: string (object id/name)
pose: PoseStamped objecct (objects pose with respect to world frame)
planning_scene_publisher: ros Publisher('/collision_object', CollisionObject)
scene: PlanningSceneInterface
robot: RobotCommander
operation: currently just use CollisionObject.ADD
"""
co = CollisionObject()
co.operation = operation
co.id = id
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def add_bin(bin, prefix="/shelf"):
# Necessary parameters
_, max_x, min_y, max_y, min_z, max_z = rospy.get_param(prefix + "/bins/" +
bin)
shelf_max_x = DEPTH / 2
shelf_min_y, shelf_max_y = -WIDTH / 2, WIDTH / 2
shelf_min_z, shelf_max_z = 0, HEIGHT
objects = []
poses = []
# Create left side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH, abs(shelf_min_y - min_y), HEIGHT],
))
poses.append(
Pose(
position=Point(x=0, y=(shelf_min_y + min_y) / 2, z=HEIGHT / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create right side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH, abs(shelf_max_y - max_y), HEIGHT],
))
poses.append(
Pose(
position=Point(x=0, y=(shelf_max_y + max_y) / 2, z=HEIGHT / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create top
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH,
abs(max_y - min_y),
abs(shelf_max_z - max_z)],
))
poses.append(
Pose(
position=Point(x=0,
y=(max_y + min_y) / 2,
z=(shelf_max_z + max_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create bottom
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH,
abs(max_y - min_y),
abs(shelf_min_z - min_z)],
))
poses.append(
Pose(
position=Point(x=0,
y=(max_y + min_y) / 2,
z=(shelf_min_z + min_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create back
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH / 2,
abs(max_y - min_y),
abs(max_z - min_z)],
))
poses.append(
Pose(
position=Point(x=shelf_max_x / 2,
y=(max_y + min_y) / 2,
z=(min_z + max_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "shelf"
co.header.frame_id = "/shelf"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
while pub.get_num_connections() < 1:
if rospy.is_shutdown():
exit(0)
rospy.logwarn("Please create a subscriber to 'obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
time.sleep(1.0)
rospy.loginfo("Continue ...")
# Publish a simple sphere
x = CollisionObject()
x.id = "Funny Sphere"
x.header.frame_id = root_frame
x.operation = CollisionObject.ADD
#x.operation = CollisionObject.REMOVE
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions.append(0.1) # radius
x.primitives.append(sphere)
pose = Pose()
pose.position.x = 0.35
pose.position.y = -0.45
pose.position.z = 0.8
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
x.primitive_poses.append(pose)
# pub.publish(x)
time.sleep(1.0)
def planAndExecuteAttached(q_map):
"""!
Fukcja planuje oraz wykonuje ruch do zadanej pozycji stawow z uwzglednieniem przenoszonego obiektu
@param q_dest slownik: Slownik {nazwa_stawu:pozycja_docelowa} zawierajacy docelowe pozycje stawow.
"""
print "Creating a virtual object attached to gripper..."
# for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject
object1 = AttachedCollisionObject()
object1.link_name = "right_HandGripLink"
object1.object.header.frame_id = "right_HandGripLink"
object1.object.id = "object1"
object1_prim = SolidPrimitive()
object1_prim.type = SolidPrimitive.CYLINDER
object1_prim.dimensions = [None, None] # set initial size of the list to 2
object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.23
object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06
object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi / 2)))
object1.object.primitives.append(object1_prim)
object1.object.primitive_poses.append(object1_pose)
object1.object.operation = CollisionObject.ADD
object1.touch_links = [
'right_HandPalmLink', 'right_HandFingerOneKnuckleOneLink',
'right_HandFingerOneKnuckleTwoLink',
'right_HandFingerOneKnuckleThreeLink',
'right_HandFingerTwoKnuckleOneLink',
'right_HandFingerTwoKnuckleTwoLink',
'right_HandFingerTwoKnuckleThreeLink',
'right_HandFingerThreeKnuckleTwoLink',
'right_HandFingerThreeKnuckleThreeLink'
]
# print "Publishing the attached object marker on topic /attached_objects"
# pub = MarkerPublisherThread(object1)
# pub.start()
print "Moving to valid position, using planned trajectory."
goal_constraint_1 = qMapToConstraints(
q_map, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(20):
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint_1],
"impedance_joints",
max_velocity_scaling_factor=0.15,
max_acceleration_scaling_factor=0.2,
planner_id="RRTConnect",
attached_collision_objects=[object1])
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(
traj, start_time=0.5, position_tol=20.0 / 180.0 * math.pi):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_map, js[1]):
print "Nie udalo sie zaplanowac ruchu do pozycji docelowej, puszka nie zostala odlozona."
exitError(6)
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_contraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
def __init__(self):
#Specify a frame_id - transformation to root_frame of obstacle_distance node is handled in according subscriber callback
self.root_frame = rospy.get_param("root_frame")
self.obstacle_frame = rospy.get_namespace() + "interactive_box_frame"
self.pub = rospy.Publisher("obstacle_distance/registerObstacle",
CollisionObject,
queue_size=1,
latch=True)
self.br = tf.TransformBroadcaster()
while self.pub.get_num_connections() < 1:
rospy.logwarn(
"Please create a subscriber to '" + rospy.get_namespace() +
"/obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)"
)
rospy.sleep(1.0)
rospy.loginfo("Continue ...")
# Compose CollisionObject (Box)
self.co = CollisionObject()
self.co.id = "Interactive Box"
self.co.header.frame_id = self.obstacle_frame
self.co.operation = CollisionObject.ADD
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions = [0.1] # extent x, y, z
self.co.primitives.append(primitive)
pose = Pose()
pose.orientation.w = 1.0
self.co.primitive_poses.append(pose)
# Compose InteractiveMarker
self.interactive_obstacle_pose = PoseStamped()
self.interactive_obstacle_pose.header.stamp = rospy.Time.now()
self.interactive_obstacle_pose.header.frame_id = self.root_frame
self.interactive_obstacle_pose.pose.position.x = -0.5
self.interactive_obstacle_pose.pose.position.y = 0.3
self.interactive_obstacle_pose.pose.position.z = 0.8
self.interactive_obstacle_pose.pose.orientation.w = 1.0
self.ia_server = InteractiveMarkerServer("obstacle_marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = self.root_frame
self.int_marker.pose = self.interactive_obstacle_pose.pose
self.int_marker.name = "interactive_obstacle_marker"
self.int_marker.scale = 0.5
# Setup MarkerControls
control_3d = InteractiveMarkerControl()
control_3d.always_visible = True
control_3d.name = "move_rotate_3D"
control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
self.int_marker.controls.append(control_3d)
control = InteractiveMarkerControl()
control.always_visible = True
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
self.ia_server.insert(self.int_marker, self.marker_fb)
self.ia_server.applyChanges()
# initial send
self.br.sendTransform(
(self.interactive_obstacle_pose.pose.position.x,
self.interactive_obstacle_pose.pose.position.y,
self.interactive_obstacle_pose.pose.position.z),
(self.interactive_obstacle_pose.pose.orientation.x,
self.interactive_obstacle_pose.pose.orientation.y,
self.interactive_obstacle_pose.pose.orientation.z,
self.interactive_obstacle_pose.pose.orientation.w),
rospy.Time.now(), self.obstacle_frame,
self.interactive_obstacle_pose.header.frame_id)
rospy.sleep(1.0)
self.pub.publish(self.co)
rospy.sleep(1.0)
def link_to_collision_object(self, link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if self.is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print(
"Element %s with geometry type %s not supported. Ignored."
% (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val)
for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(resource):
mesh_path = resource
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = pysdf.homogeneous2pose_msg(
linkpart.pose)
if not self.make_mesh(collision_object, link_pose_stamped,
mesh_path, scale):
return None
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val)
for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [float(linkpart.geometry_data['radius'])]
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(float(linkpart.geometry_data['length']),
float(linkpart.geometry_data['radius'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
return collision_object
def deliver_sample(move_arm, args):
"""
:type move_arm: class 'moveit_commander.move_group.MoveGroupCommander'
:type args: List[bool, float, float, float]
"""
move_arm.set_planner_id("RRTstar")
x_delivery = args[1]
y_delivery = args[2]
z_delivery = args[3]
# after sample collect
mypi = 3.14159
d2r = mypi / 180
r2d = 180 / mypi
goal_pose = move_arm.get_current_pose().pose
# position was found from rviz tool
goal_pose.position.x = x_delivery
goal_pose.position.y = y_delivery
goal_pose.position.z = z_delivery
r = -179
p = -20
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
# rotate scoop to deliver sample at current location...
# adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = "base_link"
pos_constraint.link_name = "l_scoop"
pos_constraint.target_point_offset.x = 0.1
pos_constraint.target_point_offset.y = 0.1
# rotate scoop to deliver sample at current location begin
pos_constraint.target_point_offset.z = 0.1
pos_constraint.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
pos_constraint.weight = 1
# using euler angles for own verification..
r = +180
p = 90 # 45 worked get
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose = move_arm.get_current_pose().pose
rotation = (goal_pose.orientation.x, goal_pose.orientation.y,
goal_pose.orientation.z, goal_pose.orientation.w)
euler_angle = euler_from_quaternion(rotation)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
move_arm.clear_pose_targets()
move_arm.set_planner_id("RRTconnect")
return True
handle_pose.position.z = 1.22
handle_pose.orientation.x = 0
handle_pose.orientation.y = 0
handle_pose.orientation.z = 0
handle_pose.orientation.w = 1
surface_pose = deepcopy(handle_pose)
surface_pose.position.x += 0.25
surface_pose.position.z += 0.11
handle_size = [0.02, 0.155, 0.16]
surface_size = [0.48, 0.23, 0.08]
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "Scoop"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
handle = SolidPrimitive()
handle.type = SolidPrimitive.BOX
handle.dimensions = handle_size
surface = SolidPrimitive()
surface.type = SolidPrimitive.BOX
surface.dimensions = surface_size
co.primitives = [handle, surface]
co.primitive_poses = [handle_pose, surface_pose]
scene._pub_co.publish(co)
print "Published scoop"
# rospy.spin()
moveit_commander.roscpp_shutdown()
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
def addObj(msg):
global objectName, closeRC, closeLC
obj = SolidPrimitive()
obj_ps = PoseStamped()
obj_ps.header.frame_id = p._fixed_frame
if msg.name == 'table':
# Reset the planning scene
p.clear()
objectName = []
# Table dimensions and pose
obj.dimensions = [
msg.dimensions[0], msg.dimensions[1], msg.center[2] + 0.65
]
obj.type = obj.BOX
obj_ps.pose.position.x = msg.center[0]
obj_ps.pose.position.y = msg.center[1]
obj_ps.pose.position.z = -0.595 + (msg.center[2] + 0.65) / 2
obj_ps.pose.orientation.x = msg.quaternion[0]
obj_ps.pose.orientation.y = msg.quaternion[1]
obj_ps.pose.orientation.z = msg.quaternion[2]
obj_ps.pose.orientation.w = msg.quaternion[3]
# Find the coordinates of the table's corner
closeRC = [
obj_ps.pose.position.x - obj.dimensions[0] / 2,
obj_ps.pose.position.y - obj.dimensions[1] / 2,
obj_ps.pose.position.z + obj.dimensions[2] / 2
]
closeLC = [
obj_ps.pose.position.x - obj.dimensions[0] / 2,
obj_ps.pose.position.y + obj.dimensions[1] / 2,
obj_ps.pose.position.z + obj.dimensions[2] / 2
]
print "The table has been detected"
if msg.name[0:8] == 'cylinder':
# Cylinder dimensions and pose
obj.dimensions = [msg.dimensions[0], msg.dimensions[1]]
obj.type = obj.CYLINDER
obj_ps.pose.position.x = msg.center[0]
obj_ps.pose.position.y = msg.center[1]
obj_ps.pose.position.z = -0.595 + 0.65 + msg.center[2]
obj_ps.pose.orientation.x = msg.quaternion[0]
obj_ps.pose.orientation.y = msg.quaternion[1]
obj_ps.pose.orientation.z = msg.quaternion[2]
obj_ps.pose.orientation.w = -msg.quaternion[3]
# Add the object to the list
if msg.name not in objectName:
objectName.append(msg.name)
print "The object", msg.name, "has been detected"
if msg.name[0:3] == 'box':
# Box dimensions and pose
obj.dimensions = [
msg.dimensions[0], msg.dimensions[1], msg.dimensions[2]
]
obj.type = obj.BOX
obj_ps.pose.position.x = msg.center[0]
obj_ps.pose.position.y = msg.center[1]
obj_ps.pose.position.z = -0.595 + 0.65 + msg.center[2]
obj_ps.pose.orientation.x = msg.quaternion[0]
obj_ps.pose.orientation.y = msg.quaternion[1]
obj_ps.pose.orientation.z = msg.quaternion[2]
obj_ps.pose.orientation.w = msg.quaternion[3]
# Add the object to the list
if msg.name not in objectName:
objectName.append(msg.name)
print "The object", msg.name, "has been detected"
# Updates the planning scene with the new object
p.addSolidPrimitive(msg.name, obj, obj_ps.pose, True)
def init_cluster_grasper(self, cluster, probabilities=[], use_probability=True):
self.cluster_frame = cluster.header.frame_id
#use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster)
if use_probability:
if len(probabilities) == 0:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, []) #empty probabilities
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_object_box', color=[0.2,0.2,1])
print 'probabilistic_object_box'
else:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
#draw the bounding box
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_weighted_object_box', color=[0,0.9,0.9])
print 'probabilistic_weighted_object_box'
else:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='deterministic_object_box', color=[1,0,0])
print 'deterministic_object_box'
# MoveIt Stuff: CollisionObject
'''
print "self.object_bounding_box_dims=", self.object_bounding_box_dims
print "self.object_bounding_box=", self.object_bounding_box
print "self.object_to_base_frame.shape=", self.object_to_base_frame.shape
print "self.object_to_base_frame=", self.object_to_base_frame
print "self.object_to_cluster_frame=", self.object_to_cluster_frame
'''
# Add the bounding box as a CollisionObject
co = CollisionObject()
co.header.stamp = rospy.get_rostime()
#co.header.frame_id = "base_footprint"
co.header.frame_id = "base_link"
co.primitives.append(SolidPrimitive())
co.primitives[0].type = SolidPrimitive.BOX
# Clear the previous CollisionObject
co.id = "part"
co.operation = co.REMOVE
self.pub_co.publish(co)
# Clear the previously attached object
aco = AttachedCollisionObject()
aco.object = co
self.pub_aco.publish(aco)
# Add the new CollisionObject
box_height = self.object_bounding_box_dims[2]
co.operation = co.ADD
co.primitives[0].dimensions.append(self.object_bounding_box_dims[0])
co.primitives[0].dimensions.append(self.object_bounding_box_dims[1])
co.primitives[0].dimensions.append(self.object_bounding_box_dims[2])
co.primitive_poses.append(Pose())
co.primitive_poses[0].position.x = self.object_to_base_frame[0,3]
co.primitive_poses[0].position.y = self.object_to_base_frame[1,3]
co.primitive_poses[0].position.z = self.object_to_base_frame[2,3] + box_height/2
quat = tf.transformations.quaternion_about_axis(math.atan2(self.object_to_base_frame[1,0], self.object_to_base_frame[0,0]), (0,0,1))
#quat = tf.transformations.quaternion_from_matrix(self.object_to_base_frame)
co.primitive_poses[0].orientation.x = quat[0]
co.primitive_poses[0].orientation.y = quat[1]
co.primitive_poses[0].orientation.z = quat[2]
co.primitive_poses[0].orientation.w = quat[3]
self.pub_co.publish(co)
# END MoveIt! stuff
#for which directions does the bounding box fit within the hand?
gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)]
self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)]
#only half benefit for the longer dimension, if one is significantly longer than the other
if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]:
if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8:
self._box_fits_in_hand[1] *= .5
elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8:
self._box_fits_in_hand[0] *= .5
#rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims))
#rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand))
#compute the average number of points per sq cm of bounding box surface (half the surface only)
bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \
(self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \
(self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100)
self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf
def motionPlanToPose(self,
pose,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm to set of joint position goals
:param joints: joint names for which the target position
is specified.
:param positions: target joint positions
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type joints: list of string element type
:type positions: list of float element type
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"max_acceleration_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("motionPlanToPose: unsupported argument: %s",
arg)
# Create goal
g = MotionPlanRequest()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.start_state = kwargs["start_state"]
except KeyError:
g.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = self._gripper_frame
# c1.position_constraints[0].target_point_offset
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose.orientation
c1.orientation_constraints[0].link_name = self._gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 7. fill in request group name
g.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.planning_time
# 10. Fill in velocity scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 11. Fill in acceleration scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"]
except KeyError:
pass # do not fill in at all
result = self._mp_service(g)
traj = result.motion_plan_response.trajectory.joint_trajectory.points
if len(traj) < 1:
rospy.logwarn('No motion plan found.')
return None
return traj
def moveToPose(self,
pose,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm, based on a goal pose_stamped for the end effector.
:param pose: target pose to which we want to move
specified link to
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type pose_stamped: ros message object of type PoseStamped
:type gripper_frame: string
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = self._gripper_frame
# c1.position_constraints[0].target_point_offset
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose.orientation
c1.orientation_constraints[0].link_name = self._gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def getGoalConstraints(self, frames, q, timeout=2.0, mode=ModeJoints):
is_list_of_frame = isinstance(frames, list)
number_of_frames = None
if len(self.robot_ns) > 0:
srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik",
moveit_msgs.srv.GetPositionIK)
else:
srv = rospy.ServiceProxy("compute_ik",
moveit_msgs.srv.GetPositionIK)
goal = Constraints()
if is_list_of_frame:
number_of_frames = len(frames)
else:
number_of_frames = 1
# print "Start looping thru cartesian points"
for i in xrange(number_of_frames):
frame = None
joints = None
previous_q = None
if is_list_of_frame:
print "is a list of frames"
frame = frames[i]
else:
"just one frame"
frame = frames
if i == 0:
previous_q = q
joints = self.ik(pm.toMatrix(frame), previous_q)
if joints is not None:
previous_q = joints
else:
return (None, None)
# print joints is None
# print joints
# if joints is None:
# p = geometry_msgs.msg.PoseStamped()
# p.pose.position.x = frame.position.x
# p.pose.position.y = frame.position.y
# p.pose.position.z = frame.position.z
# p.pose.orientation.x = frame.orientation.x
# p.pose.orientation.y = frame.orientation.y
# p.pose.orientation.z = frame.orientation.z
# p.pose.orientation.w = frame.orientation.w
# p.header.frame_id = "/world"
# ik_req = moveit_msgs.msg.PositionIKRequest()
# ik_req.robot_state.joint_state.name = self.joint_names
# ik_req.robot_state.joint_state.position = q
# ik_req.avoid_collisions = True
# ik_req.timeout = rospy.Duration(timeout)
# ik_req.attempts = 5
# ik_req.group_name = self.group
# ik_req.pose_stamped = p
# rospy.logwarn("Getting IK position...")
# ik_resp = srv(ik_req)
# rospy.logwarn("IK RESULT ERROR CODE = %d"%(ik_resp.error_code.val))
# #if ik_resp.error_code.val > 0:
# # return (ik_resp, None)
# #print ik_resp.solution
# ###############################
# # now create the goal based on inverse kinematics
# if not ik_resp.error_code.val < 0:
# for i in range(0,len(ik_resp.solution.joint_state.name)):
# print ik_resp.solution.joint_state.name[i]
# print ik_resp.solution.joint_state.position[i]
# joint = JointConstraint()
# joint.joint_name = ik_resp.solution.joint_state.name[i]
# joint.position = ik_resp.solution.joint_state.position[i]
# joint.tolerance_below = 0.005
# joint.tolerance_above = 0.005
# joint.weight = 1.0
# goal.joint_constraints.append(joint)
# return (ik_resp, goal)
if mode == ModeJoints:
for i in range(0, len(self.joint_names)):
joint = JointConstraint()
joint.joint_name = self.joint_names[i]
joint.position = joints[i]
joint.tolerance_below = 0.005
joint.tolerance_above = 0.005
joint.weight = 1.0
goal.joint_constraints.append(joint)
else:
print 'Cartesian Constraint', self.base_link, self.end_link
# TODO: Try to fix this again. Something is wrong
position_costraint = PositionConstraint()
position_costraint.link_name = self.end_link
position_costraint.target_point_offset.x = 0.0
position_costraint.target_point_offset.y = 0.0
position_costraint.target_point_offset.z = 0.0
sphere_bounding = SolidPrimitive()
sphere_bounding.type = sphere_bounding.SPHERE
sphere_bounding.dimensions.append(0.005)
position_costraint.constraint_region.primitives.append(
sphere_bounding)
sphere_pose = Pose()
sphere_pose.position = frame.p
sphere_pose.orientation.x = 0.0
sphere_pose.orientation.y = 0.0
sphere_pose.orientation.z = 0.0
sphere_pose.orientation.w = 1.0
position_costraint.constraint_region.primitive_poses.append(
sphere_pose)
position_costraint.weight = 1.0
position_costraint.header.frame_id = '/world'
goal.position_constraints.append(position_costraint)
orientation_costraint = OrientationConstraint()
orientation_costraint.link_name = self.end_link
orientation_costraint.header.frame_id = '/world'
orientation_costraint.orientation = frame.M.GetQuaternion()
orientation_costraint.absolute_x_axis_tolerance = 0.005
orientation_costraint.absolute_y_axis_tolerance = 0.005
orientation_costraint.absolute_z_axis_tolerance = 0.005
orientation_costraint.weight = 1.0
goal.orientation_constraints.append(orientation_costraint)
return (None, goal)
def graspAndApproach(collisionObject, arm):
objectType = SolidPrimitive()
GRIP_SIZE = 0.1 # size of the open grippers
SHIFT = 0.10 # distance between approach and grasp
graspList = []
approachList = []
# The possible grasp depends on the type of the object (BOX or CYLINDER)
if collisionObject.primitives[0].type == objectType.CYLINDER:
# Poses depends on the arm group used
if arm == gr:
testPoses = rightPoses
elif arm == gl:
testPoses = leftPoses
# If the diameter of the cylinder is smaller than the size of the grippers
if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE:
for quat in testPoses:
grasp = PoseStamped()
grasp.header = deepcopy(collisionObject.header)
grasp.pose = deepcopy(collisionObject.primitive_poses[0])
grasp.pose.orientation = quat
# Fine tuning of the grasp position
if quat == right_side:
grasp.pose.position.x = 0.97 * grasp.pose.position.x
grasp.pose.position.y += min(
(collisionObject.primitives[0].dimensions[1]), 0.05
) #account for object being too tall most of the time
grasp.pose.position.z -= 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
approach = deepcopy(grasp)
approach.pose.position.y -= SHIFT
elif quat == left_side:
grasp.pose.position.z -= 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
grasp.pose.position.y -= min(
(collisionObject.primitives[0].dimensions[1]), 0.05
) #account for object being too tall most of the time
approach = deepcopy(grasp)
approach.pose.position.y += SHIFT
elif quat == front:
grasp.pose.position.y = 0.975 * grasp.pose.position.y
approach = deepcopy(grasp)
approach.pose.position.x -= SHIFT
elif quat == frontOriented:
grasp.pose.position.y = 0.975 * grasp.pose.position.y
grasp.pose.position.z -= 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
approach = deepcopy(grasp)
approach.pose.position.x -= SHIFT
elif quat == top:
grasp.pose.position.z += collisionObject.primitives[
0].dimensions[0] / 2 - 0.08
grasp.pose.position.x = 0.97 * grasp.pose.position.x
if grasp.pose.position.y < 0:
grasp.pose.position.y = 0.96 * grasp.pose.position.y
else:
grasp.pose.position.y = 1.04 * grasp.pose.position.y
approach = deepcopy(grasp)
approach.pose.position.z += SHIFT
graspList.append(grasp)
approachList.append(approach)
# Box grasp is not fined tuned yet !
elif collisionObject.primitives[0].type == objectType.BOX:
grasp_top = PoseStamped()
grasp_top.header = deepcopy(collisionObject.header)
grasp_top.pose = deepcopy(collisionObject.primitive_poses[0])
grasp_side = deepcopy(grasp_top)
grasp_front = deepcopy(grasp_top)
# Orientation of the object in the base frame
quat = [
grasp_top.pose.orientation.x, grasp_top.pose.orientation.y,
grasp_top.pose.orientation.z, grasp_top.pose.orientation.w
]
euler = euler_from_quaternion(quat)
yaw = -(euler[2] + pi / 2)
# If the object is small enough to be grasped
if collisionObject.primitives[0].dimensions[0] <= GRIP_SIZE:
# top
quat_top = quaternion_from_euler(pi, 0.0, yaw, axes='rxyz')
top_box.x = quat_top[0]
top_box.y = quat_top[1]
top_box.z = quat_top[2]
top_box.w = quat_top[3]
grasp_top.pose.position.z += 0.5 * collisionObject.primitives[
0].dimensions[
2] - 0.08 #account for object being too tall most of the time
grasp_top.pose.position.x = 0.98 * grasp_top.pose.position.x
grasp_top.pose.position.y = 0.9 * grasp_top.pose.position.y
grasp_top.pose.orientation = top_box
approach_top = deepcopy(grasp_top)
approach_top.pose.position.z += SHIFT
graspList.append(grasp_top)
approachList.append(approach_top)
# side
quat_side = quaternion_from_euler(pi, yaw, pi / 2, axes='rxzy') #
box_side.x = quat_side[0]
box_side.y = quat_side[1]
box_side.z = quat_side[2]
box_side.w = quat_side[3]
grasp_side.pose.orientation = box_side
grasp_top.pose.position.x = 0.95 * grasp_top.pose.position.x
grasp_top.pose.position.y = 0.95 * grasp_top.pose.position.y
approach_side = deepcopy(grasp_side)
euler_side = euler_from_quaternion(quat_side)
angle_side = euler_side[0]
approach_side.pose.position.x += SHIFT * sin(angle_side)
approach_side.pose.position.y += SHIFT * cos(angle_side)
graspList.append(grasp_side)
approachList.append(approach_side)
if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE:
# front
quat_front = quaternion_from_euler(pi,
yaw - pi / 2,
pi / 2,
axes='rxzy')
box_front.x = quat_front[0]
box_front.y = quat_front[1]
box_front.z = quat_front[2]
box_front.w = quat_front[3]
grasp_front.pose.orientation = box_front
grasp_top.pose.position.x = 0.96 * grasp_top.pose.position.x
grasp_top.pose.position.y = 0.96 * grasp_top.pose.position.y
approach_front = deepcopy(grasp_front)
euler_front = euler_from_quaternion(quat_front)
angle_front = euler_front[0]
approach_front.pose.position.x += SHIFT * sin(angle_front)
approach_front.pose.position.y += SHIFT * cos(angle_front)
graspList.append(grasp_front)
approachList.append(approach_front)
return graspList, approachList
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# TODO check userdata
# if not isinstance(userdata.pose, PoseStamped):
#Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
#self._planning_failed = True
#return
check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)
# request planing and execution
action_goal = MoveGroupGoal()
# set palnning options
action_goal.planning_options.plan_only = self._plan_only
action_goal.planning_options.replan = False
# action_goal.planning_options.planning_scene_diff.is_diff = True
# action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# setup request
action_goal.request.group_name = self._move_group
action_goal.request.num_planning_attempts = 3
action_goal.request.allowed_planning_time = 1.0
action_goal.request.max_velocity_scaling_factor = 1.0
action_goal.request.max_acceleration_scaling_factor = 1.0
# start pose
action_goal.request.start_state.is_diff = True
# pose constraint
goal_constraint = Constraints(name='')
# set target position
constraint = PositionConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.constraint_region = BoundingVolume()
constraint.constraint_region.primitives = [
SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[self._position_tolerance])
]
constraint.constraint_region.primitive_poses = [
Pose(position=userdata.pose.pose.position)
]
constraint.weight = 1.0
goal_constraint.position_constraints.append(constraint)
# set target orientation
constraint = OrientationConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.orientation = userdata.pose.pose.orientation
constraint.absolute_x_axis_tolerance = self._orientation_tolerance
constraint.absolute_y_axis_tolerance = self._orientation_tolerance
constraint.absolute_z_axis_tolerance = self._orientation_tolerance
constraint.weight = 0.1
goal_constraint.orientation_constraints.append(constraint)
# add goal_constraint
action_goal.request.goal_constraints.append(goal_constraint)
try:
self._client.send_goal('move_group', action_goal)
except Exception as e:
Logger.logwarn(
'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
% (self._move_group, str(e)))
self._planning_failed = True
def urdf_to_payload(xml_str):
urdf_robot = URDF.from_xml_string(xml_str)
urdf_et = ET.fromstring(xml_str)
base_links = [
u for u in urdf_robot.child_map.keys()
if u not in urdf_robot.parent_map
]
assert len(base_links) == 1, "Multiple payloads detected, invalid URDF."
base_link = base_links[0]
assert base_link not in urdf_robot.link_map, "Invalid initial_pose parent link in payload URDF"
assert len(urdf_robot.child_map[base_link]
) == 1, "Invalid initial_pose in payload URDF"
(initial_pose_joint_name,
payload_link_name) = urdf_robot.child_map[base_link][0]
assert initial_pose_joint_name.endswith(
'_initial_pose'), "Invalid initial_pose parent link in payload URDF"
#assert all([j.type == "fixed" for _, j in urdf_robot.joint_map.items()]), "All joints must be fixed type in payload URDF"
assert all([urdf_robot.parent_map[l][1] == payload_link_name for l in urdf_robot.link_map if l != payload_link_name]), \
"All links must have payload link as parent"
payload_link = urdf_robot.link_map[payload_link_name]
initial_pose_joint = urdf_robot.joint_map[initial_pose_joint_name]
payload_msg = Payload()
payload_msg.name = payload_link_name
#Load in initial pose
payload_msg.header.frame_id = initial_pose_joint.parent
payload_msg.pose = _origin_to_pose(initial_pose_joint.origin)
#Load in inertia
if payload_link.inertial is not None:
m = Inertia()
m.m = payload_link.inertial.mass
m.ixx = payload_link.inertial.inertia.ixx
m.ixy = payload_link.inertial.inertia.ixy
m.ixz = payload_link.inertial.inertia.ixz
m.iyy = payload_link.inertial.inertia.iyy
m.iyz = payload_link.inertial.inertia.iyz
m.izz = payload_link.inertial.inertia.izz
if payload_link.inertial.origin is not None:
if payload_link.inertial.origin.xyz is not None:
m.com.x = payload_link.inertial.origin.xyz[0]
m.com.y = payload_link.inertial.origin.xyz[1]
m.com.z = payload_link.inertial.origin.xyz[2]
if payload_link.inertial.origin.rpy is not None:
R = np.matrix(
rox.rpy2R(np.array(payload_link.inertial.origin.rpy)))
I = np.matrix([[m.ixx, m.ixy, m.ixz], [m.ixy, m.iyy, m.iyz],
[m.ixz, m.iyz, m.izz]])
I2 = np.dot(np.transpose(R), I).dot(R)
m.ixx = I2[0, 0]
m.ixy = I2[0, 1]
m.ixz = I2[0, 2]
m.ixy = I2[1, 0]
m.iyy = I2[1, 1]
m.iyz = I2[1, 2]
m.izz = I2[2, 2]
payload_msg.inertia = m
#Load in gripper targets
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_gripper_target(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid gripper_target for payload in URDF"
pose = _origin_to_pose(j.origin)
gripper_target = GripperTarget()
gripper_target.header.frame_id = payload_link_name
gripper_target.name = l.name
gripper_target.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
ft_et = link_et.find('.//gripper_ft_threshold')
if ft_et is not None:
pickup_et = ft_et.find('.//pickup')
if pickup_et is not None:
ft_val = [float(d) for d in pickup_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.pickup_ft_threshold = ft_val
place_et = ft_et.find('.//place')
if place_et is not None:
ft_val = [float(d) for d in place_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.place_ft_threshold = ft_val
payload_msg.gripper_targets.append(gripper_target)
#Load in markers
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_marker(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid marker for payload in URDF"
pose = _origin_to_pose(j.origin)
aruco_marker = next((x for x in payload_msg.markers if x.name == l),
None)
aruco_marker = ArucoGridboardWithPose()
aruco_marker.header.frame_id = payload_link_name
aruco_marker.name = l.name
aruco_marker.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
marker_et = link_et.find('.//aruco_marker')
if marker_et is not None:
gridboard_et = marker_et.find('.//gridboard')
if gridboard_et is not None:
a = gridboard_et.attrib
aruco_marker.marker.markersX = int(a['markersX'])
aruco_marker.marker.markersY = int(a['markersY'])
aruco_marker.marker.markerLength = float(a['markerLength'])
aruco_marker.marker.markerSpacing = float(a['markerSpacing'])
aruco_marker.marker.dictionary = a['dictionary']
aruco_marker.marker.firstMarker = int(a['firstMarker'])
payload_msg.markers.append(aruco_marker)
#Load in meshes
payload_geometry_chain = [(None, payload_link_name)]
if payload_link_name in urdf_robot.child_map:
payload_geometry_chain.extend(urdf_robot.child_map[payload_link_name])
for j_name, l_name in payload_geometry_chain:
#v=urdf_robot.link_map[l_name].visual
j = urdf_robot.joint_map[j_name] if j_name is not None else None
link_et = urdf_et.find('.//link[@name="' + l_name + '"]')
#Workaround to handle multiple visual elements
visual_et_s = link_et.findall('.//visual')
for visual_et in visual_et_s:
v = Visual.from_xml(visual_et)
if j is not None:
visual_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
visual_pose = _origin_to_pose(v.origin)
if v.material is None or v.material.color is None:
visual_color = ColorRGBA(0.5, 0.5, 0.5, 1)
else:
visual_color = ColorRGBA(*v.material.color.rgba)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.visual_geometry.mesh_poses.append(visual_pose)
payload_msg.visual_geometry.mesh_resources.append(mesh_fname)
payload_msg.visual_geometry.mesh_scales.append(mesh_scale)
payload_msg.visual_geometry.mesh_colors.append(visual_color)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
else:
assert False, "Invalid geometry in URDF"
#Workaround to handle multiple collision elements
collision_et_s = link_et.findall('.//collision')
for collision_et in collision_et_s:
v = Collision.from_xml(collision_et)
if j is not None:
collision_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
collision_pose = _origin_to_pose(v.origin)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.collision_geometry.mesh_poses.append(
collision_pose)
payload_msg.collision_geometry.mesh_resources.append(
mesh_fname)
payload_msg.collision_geometry.mesh_scales.append(mesh_scale)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
else:
assert False, "Invalid geometry in URDF"
payload_msg.confidence = 0.1
#TODO: read inertial values
#Sanity check
payload_msg._check_types()
return payload_msg
TABLE_CENTER = PoseStamped()
TABLE_CENTER.header.frame_id = 'world'
TABLE_CENTER.pose = Pose(position=Point(0, -1.37, .76))
TABLE_CENTER.pose.orientation.w = 1
TABLE_SIZE = [1.525, 2.74, 0.0201]
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
print(co)
count = 0
while not rospy.is_shutdown():
pub.publish(co)
co.primitive_poses[0].position.x += 1
count += 1
print(count)
