def DetectBins(self, robot, table, **kw_args):
"""
Place bins on table
"""
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
env = robot.GetEnv()
# Place blocks in a pattern on the table
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
table_height = table_aabb.pos()[2] + table_aabb.extents()[2]
table_corner = numpy.eye(4)
table_corner[:3, 3] = [
table_aabb.pos()[0] - table_aabb.extents()[0],
table_aabb.pos()[1] - table_aabb.extents()[1],
table_height,
]
# Get the pr-ordata package path
data_dir = prpy.util.FindCatkinResource("pr_ordata", "data")
# Import the yaml file telling where to place blocks
bin_config_dir = prpy.util.FindCatkinResource("block_sorting", "config")
bin_config = os.path.join(bin_config_dir, "bin_metadata.yaml")
with open(bin_config, "r") as f:
import yaml
bin_yaml = yaml.load(f.read())
bins = []
for b in bin_yaml:
bin_obj = env.ReadKinBodyXMLFile(os.path.join(data_dir, "objects", b["kinbody"]))
# Set the pose
bin_pose = numpy.eye(4)
bin_pose[:2, 3] = b["pose"]
bin_in_world = numpy.dot(table_corner, bin_pose)
bin_in_world[2, 3] = table_height
bin_obj.SetTransform(bin_in_world)
# Set the color
for l in bin_obj.GetLinks():
for g in l.GetGeometries():
color = b["color"] + [1.0]
g.SetDiffuseColor(numpy.array(color))
# Set the name
bin_obj.SetName(b["name"])
# Add it
env.Add(bin_obj)
bins.append(bin_obj)
return bins
def DetectBlocks(self, robot, table, blocks=[], **kw_args):
"""
Place blocks on the table
"""
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
if len(blocks) == 0:
# Add all blocks
# Import the yaml file telling where to place blocks
block_config_dir = prpy.util.FindCatkinResource("block_sorting", "config")
block_config = os.path.join(block_config_dir, "block_pattern.yaml")
with open(block_config, "r") as f:
import yaml
block_yaml = yaml.load(f.read())
env = robot.GetEnv()
# Place blocks in a pattern on the table
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
table_height = table_aabb.pos()[2] + table_aabb.extents()[2]
table_corner = numpy.eye(4)
table_corner[:3, 3] = [
table_aabb.pos()[0] - table_aabb.extents()[0],
table_aabb.pos()[1] - table_aabb.extents()[1],
table_height,
]
for b in block_yaml:
block = env.ReadKinBodyXMLFile("objects/block.kinbody.xml")
block_pose = numpy.eye(4)
block_pose[:2, 3] = b["pose"]
block_in_world = numpy.dot(table_corner, block_pose)
block_in_world[2, 3] = table_height
block.SetTransform(block_in_world)
block.SetName(b["name"])
env.Add(block)
blocks.append(block)
return blocks
def _GrabBlock(robot, blocks, table, manip=None, preshape=None,
**kw_args):
"""
@param robot The robot performing the grasp
@param block The block to grab
@param table The table, or object, the block is resting on
@param manip The manipulator to use to grab the block
(if None active manipulator is used)
@param preshape The shape (dof_values) to move the hand to before
executing the grab
"""
from prpy.rave import AllDisabled, Disabled
from prpy.viz import RenderTSRList, RenderVector
env = robot.GetEnv()
block = None
if manip is None:
with env:
manip = robot.GetActiveManipulator()
if preshape is None:
preshape=[1.7, 1.7, 0.2, 2.45]
# First move the hand to the right preshape
manip.hand.MoveHand(*preshape)
block_tsr_list = []
with env:
for b in blocks:
# Get a TSR to move near the block.
tsr_list = robot.tsrlibrary(b, 'grasp', manip=manip)
block_tsr_list += tsr_list
# Plan to a pose above the block
with RenderTSRList(block_tsr_list, robot.GetEnv()):
with Disabled(table, padding_only=True):
manip.PlanToTSR(block_tsr_list, execute=True)
with manip.GetRobot().GetEnv():
ee_pose = manip.GetEndEffectorTransform()
block_idxs = [ idx for idx, tsr_chain in enumerate(block_tsr_list)
if tsr_chain.contains(ee_pose) ]
if len(block_idxs) == 0:
raise NoTSRException("Failed to find the TSR PlanToTSR planned to")
block = blocks[block_idxs[0]]
try:
with AllDisabled(env, [table] + blocks, padding_only=True):
# Move down until touching the table
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
table_aabb = ComputeEnabledAABB(table)
table_height = table_aabb.pos()[2] + table_aabb.extents()[2]
# 0.14 is the distance from finger tip to end-effector frame
current_finger_height = manip.GetEndEffectorTransform()[2,3] - 0.14
min_distance = 0.10# current_finger_height - table_height
down_direction = [0., 0., -1.]
with RenderVector(start_point, down_direction, min_distance, env):
manip.MoveUntilTouch(direction=down_direction, timelimit=5,
distance=min_distance, max_distance=min_distance + 0.05,
ignore_collisions=blocks + [table])
# Move parallel to the table to funnel the block into the fingers
# by projecting the -x direction of end-effector onto the xy-plane.
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
funnel_direction = -1. * manip.GetEndEffectorTransform()[:3,0]
funnel_direction[2] = 0.
# TODO: We should only have to disable the block for this. Why does
# this fail if we do not disable the table?
with AllDisabled(env, blocks + [table]):
with RenderVector(start_point, funnel_direction, 0.1, env):
manip.PlanToEndEffectorOffset(direction=funnel_direction,
distance=0.08, max_distance=0.12,
timelimit=5., execute=True)
# Close the finger to grab the block
manip.hand.MoveHand(f3=1.7)
# Compute the pose of the block in the hand frame
with env:
local_p = [0.01, 0, 0.24, 1.0]
hand_pose = manip.GetEndEffectorTransform()
world_p = numpy.dot(hand_pose, local_p)
block_pose = block.GetTransform()
block_pose[:,3] = world_p
block_relative = numpy.dot(numpy.linalg.inv(hand_pose), block_pose)
# Now lift the block up off the table
with AllDisabled(env, blocks + [table]):
manip.PlanToEndEffectorOffset(direction=[0, 0, 1], distance=0.05,
timelimit=5, execute=True)
# OpenRAVE trick to hallucinate the block into the correct pose relative to the hand
with env:
hand_pose = manip.GetEndEffectorTransform()
block_pose = numpy.dot(hand_pose, block_relative)
block.SetTransform(block_pose)
manip.GetRobot().Grab(block)
return block
except PlanningError as e:
logger.error('Failed to complete block grasp')
raise
def PushToPoseOnTable(robot, obj, table, goal_position, goal_radius,
manip=None, max_plan_duration=30.0,
shortcut_time=3., render=True, **kw_args):
"""
@param robot The robot performing the push
@param obj The object to push
@param table The table kinbody the object is resting on
@param goal_position The desired (x,y) position of the object in world coordinates
@param goal_radius The max distance from goal_position for the object to be to
still consider the goal achieved
@param manip The manipulator to use for the push - if None the active manipulator is used
@param max_plan_duration The max time to run the planner
@param shortcut_time The amount of time to spend shortcutting, if 0. no shortcutting is performed
@param render If true, render the trajectory while executing
"""
# Get a push planner
try:
from or_pushing.push_planner import PushPlanner
planner = PushPlanner(robot.GetEnv())
except ImportError:
raise ActionError("Unable to create PushPlanner. Is the randomized_rearrangement_planning"
"repository checked out in your workspace?")
# Get the manipulator
if manip is None:
with robot.GetEnv():
manip = robot.GetActiveManipulator()
with robot.GetEnv():
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
ee_pushing_transform = manip.GetEndEffectorTransform()
goal_pose = obj.GetTransform()
# Make the state bounds be at the edges of the table
table_pos = table_aabb.pos()
table_extents = table_aabb.extents()
sbounds = {'high': [table_pos[0] + table_extents[0],
table_pos[1] + table_extents[1],
2.*numpy.pi],
'low': [table_pos[0] - table_extents[0],
table_pos[1] - table_extents[1],
0]}
# Assume we want to keep the current orientation and height of the manipulator
# throughout the push
ee_pushing_transform[:2,3] = [0., 0.] #ignore x,y pose
# Compute the goal pose
table_height = table_pos[2] + table_extents[2]
goal_pose[:3,3] = [goal_position[0],
goal_position[1],
table_height]
with robot.CreateRobotStateSaver():
traj = planner.PushToPose(robot, obj, goal_pose,
state_bounds = sbounds,
pushing_manifold = ee_pushing_transform.flatten().tolist(),
max_plan_duration = max_plan_duration,
goal_epsilon = goal_radius,
**kw_args)
if traj is None:
raise PlanningError('Failed to find pushing plan')
# Execute
from prpy.viz import RenderTrajectory
with RenderTrajectory(robot, traj, color=[1, 0, 0, 1], render=render):
if shortcut_time > 0:
traj = planner.ShortcutPath(timelimit=shortcut_time)
with RenderTrajectory(robot, traj, color=[0, 0, 1, 1], render=render):
if manip.simulated:
# Use the push planner code to simulate path execution.
# This simulates the pushing of the objects during
# execution of the trajectory.
planner.ExecutePlannedPath()
else:
# Execute the trajectory
robot.ExecuteTrajectory(traj)
# During execution, object pushes won't be simulated.
# In the OpenRAVE world, the robot will instead move through the objects
# and probably be in collision at the end.
# This call sets all the objects to their expected poses
# at the end of the trajectory. If execution was successful, this should resolve
# collisions.
planner.SetFinalObjectPoses()
return traj
def plan(self, env):
"""
Plan to rearrange objects.
@param env: OpenRAVE environment
@return a RearrangeObjectSolution
@throws an ActionError if the or_pushing module is missing, or if an
error is encountered during planning
"""
if not HAS_RANDOMIZED_REARRANGEMENT_PLANNING:
raise ActionError(
"Unable to import or_pushing. Is the randomized_rearrangement_planning"
"repository checked out in your workspace?")
on_obj = self.get_on_obj(env)
manipulator = self.get_manipulator(env)
robot = self.get_robot(env)
movables = self.get_movables(env)
obj = self.get_pushed_object(env)
# Make the state bounds be at the edges of the table
with env:
on_obj_aabb = ComputeEnabledAABB(on_obj)
on_obj_pos = on_obj_aabb.pos()
on_obj_extents = on_obj_aabb.extents()
sbounds = {
'high': [
on_obj_pos[0] + on_obj_extents[0],
on_obj_pos[1] + on_obj_extents[1], 2. * np.pi
],
'low': [
on_obj_pos[0] - on_obj_extents[0],
on_obj_pos[1] - on_obj_extents[1], 0
]
}
# Assume we want to keep the current orientation and heigh of the manipulator
# throughout the push
with env:
ee_pushing_transform = manipulator.GetTransform()
ee_pushing_transform[:2, 3] = [0., 0.] # ignore x,y pose
planner = DiscreteSearchPushPlanner(env)
try:
with robot.CreateRobotStateSaver():
robot.SetActiveManipulator(manipulator)
traj = planner.PushToPose(
robot,
obj,
self.goal_pose,
state_bounds=sbounds,
movable_objects=movables,
pushing_model='quasistatic',
pushing_manifold=ee_pushing_transform.flatten().tolist(),
goal_radius=self.goal_epsilon,
**self.kwargs)
self.ompl_path = planner.GetPlannedPath()
self.planner_params = planner.GetPlannerParameters()
# Mark this action as deterministic based on the traj tags
path_tags = GetTrajectoryTags(path)
deterministic = path_tags.get(Tags.DETERMINISTIC_ENDPOINT, None)
if deterministic is None:
LOGGER.warn(
"Trajectory does not have DETERMINISTIC_ENDPOINT flag set. "
"Assuming non-deterministic.")
deterministic = False
except PlanningError as err:
filepath = '/tmp'
planner.WritePlannerData(filepath)
raise ActionError(str(err), deterministic=err.deterministic)
if traj is None:
raise ActionError('Failed to find pushing plan',
deterministic=deterministic)
return RearrangeObjectSolution(action=self,
traj=traj,
deterministic=deterministic)
env.Add(glass)
glass_transform = numpy.eye(4)
glass_transform[:3, 3] = [0.6239455840637041, -0.4013916328109689, 0.75]
glass.SetTransform(glass_transform)
# Goal pose for the object
import numpy
goal_in_table = [0.15, 0., -0.03, 1.]
goal_in_world = numpy.dot(table.GetTransform(), goal_in_table)
goal_pose = goal_in_world[:2]
goal_radius = 0.1
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
table_height = 0.01 + table_aabb.pos()[2] + table_aabb.extents()[2]
pts = []
for a in numpy.arange(0, 2.*numpy.pi, 0.1):
pts.append([goal_pose[0] + goal_radius*numpy.sin(a),
goal_pose[1] + goal_radius*numpy.cos(a),
table_height])
h = env.drawlinestrip(points=numpy.array(pts), linewidth=5.0,
colors=numpy.array([0.0, 1., 0.]))
# Plan to the start configuration
try:
push_arm.SetActive()
start_pose = [4.49119545, -1.59899798, -0.6, 1.65274406, -1.7742985, -0.63854765, -1.23051631]
push_arm.PlanToConfiguration(start_pose, execute=True)
def _GrabBlock(robot, blocks, table, manip=None, preshape=None, **kw_args):
"""
@param robot The robot performing the grasp
@param block The block to grab
@param table The table, or object, the block is resting on
@param manip The manipulator to use to grab the block
(if None active manipulator is used)
@param preshape The shape (dof_values) to move the hand to before
executing the grab
"""
from prpy.rave import AllDisabled, Disabled
from prpy.viz import RenderTSRList, RenderVector
env = robot.GetEnv()
block = None
if manip is None:
with env:
manip = robot.GetActiveManipulator()
if preshape is None:
preshape = [1.7, 1.7, 0.2, 2.45]
# First move the hand to the right preshape
manip.hand.MoveHand(*preshape)
block_tsr_list = []
with env:
for b in blocks:
# Get a TSR to move near the block.
tsr_list = robot.tsrlibrary(b, 'grasp', manip=manip)
block_tsr_list += tsr_list
# Plan to a pose above the block
with RenderTSRList(block_tsr_list, robot.GetEnv()):
with Disabled(table, padding_only=True):
manip.PlanToTSR(block_tsr_list, execute=True)
with manip.GetRobot().GetEnv():
ee_pose = manip.GetEndEffectorTransform()
block_idxs = [
idx for idx, tsr_chain in enumerate(block_tsr_list)
if tsr_chain.contains(ee_pose)
]
if len(block_idxs) == 0:
raise NoTSRException("Failed to find the TSR PlanToTSR planned to")
block = blocks[block_idxs[0]]
try:
with AllDisabled(env, [table] + blocks, padding_only=True):
# Move down until touching the table
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
table_aabb = ComputeEnabledAABB(table)
#table_height = table_aabb.pos()[2] + table_aabb.extents()[2]
# 0.14 is the distance from finger tip to end-effector frame
#current_finger_height = manip.GetEndEffectorTransform()[2, 3] - 0.14
min_distance = 0.10 # current_finger_height - table_height
down_direction = [0., 0., -1.]
with RenderVector(start_point, down_direction, min_distance, env):
manip.MoveUntilTouch(direction=down_direction,
timelimit=5,
distance=min_distance,
max_distance=min_distance + 0.05,
ignore_collisions=blocks + [table])
# Move parallel to the table to funnel the block into the fingers
# by projecting the -x direction of end-effector onto the xy-plane.
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
funnel_direction = -1. * manip.GetEndEffectorTransform()[:3, 0]
funnel_direction[2] = 0.
# TODO: We should only have to disable the block for this. Why does
# this fail if we do not disable the table?
with AllDisabled(env, blocks + [table]):
with RenderVector(start_point, funnel_direction, 0.1, env):
manip.PlanToEndEffectorOffset(direction=funnel_direction,
distance=0.08,
max_distance=0.12,
timelimit=5.,
execute=True)
# Close the finger to grab the block
manip.hand.MoveHand(f3=1.7)
# Compute the pose of the block in the hand frame
with env:
local_p = [0.01, 0, 0.24, 1.0]
hand_pose = manip.GetEndEffectorTransform()
world_p = numpy.dot(hand_pose, local_p)
block_pose = block.GetTransform()
block_pose[:, 3] = world_p
block_relative = numpy.dot(numpy.linalg.inv(hand_pose), block_pose)
# Now lift the block up off the table
with AllDisabled(env, blocks + [table]):
manip.PlanToEndEffectorOffset(direction=[0, 0, 1],
distance=0.05,
timelimit=5,
execute=True)
# OpenRAVE trick to hallucinate the block into the correct
# pose relative to the hand
with env:
hand_pose = manip.GetEndEffectorTransform()
block_pose = numpy.dot(hand_pose, block_relative)
block.SetTransform(block_pose)
manip.GetRobot().Grab(block)
return block
except PlanningError as e:
logger.error('Failed to complete block grasp')
raise
def PushToPoseOnTable(robot,
obj,
table,
goal_position,
goal_radius,
manip=None,
max_plan_duration=30.0,
shortcut_time=3.,
render=True,
search=True,
tracker=None,
**kw_args):
"""
@param robot The robot performing the push
@param obj The object to push
@param table The table kinbody the object is resting on
@param goal_position The desired (x,y) position of the object in
world coordinates
@param goal_radius The max distance from goal_position for the object
to be to still consider the goal achieved
@param manip The manipulator to use for the push - if None the active
manipulator is used
@param max_plan_duration The max time to run the planner
@param shortcut_time The amount of time to spend shortcutting,
if 0. no shortcutting is performed
@param render If true, render the trajectory while executing
@param tracker The perception module to use to track the object
during pushing
"""
# Get a push planner
try:
if search:
from or_pushing.discrete_search_push_planner import DiscreteSearchPushPlanner
planner = DiscreteSearchPushPlanner(robot.GetEnv())
else:
from or_pushing.push_planner import PushPlanner
planner = PushPlanner(robot.GetEnv())
except ImportError:
raise ActionError(
"Unable to create PushPlanner. Is the randomized_rearrangement_planning"
"repository checked out in your workspace?")
# Get the manipulator
if manip is None:
with robot.GetEnv():
manip = robot.GetActiveManipulator()
with robot.GetEnv():
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
ee_pushing_transform = manip.GetEndEffectorTransform()
# Make the state bounds be at the edges of the table
table_pos = table_aabb.pos()
table_extents = table_aabb.extents()
sbounds = {
'high': [
table_pos[0] + table_extents[0], table_pos[1] + table_extents[1],
2. * numpy.pi
],
'low':
[table_pos[0] - table_extents[0], table_pos[1] - table_extents[1], 0]
}
# Assume we want to keep the current orientation and height of the
# manipulator throughout the push
ee_pushing_transform[:2, 3] = [0., 0.] #ignore x,y pose
# Compute the goal pose
#table_height = table_pos[2] + table_extents[2]
goal_pose = [goal_position[0], goal_position[1]]
with robot.CreateRobotStateSaver():
traj = planner.PushToPose(
robot,
obj,
goal_pose,
state_bounds=sbounds,
pushing_manifold=ee_pushing_transform.flatten().tolist(),
max_plan_duration=max_plan_duration,
goal_radius=goal_radius,
**kw_args)
if traj is None:
raise PlanningError('Failed to find pushing plan')
# Execute
from prpy.viz import RenderTrajectory
with RenderTrajectory(robot, traj, color=[1, 0, 0, 1], render=render):
if shortcut_time > 0:
traj = planner.ShortcutPath(timelimit=shortcut_time)
with RenderTrajectory(robot, traj, color=[0, 0, 1, 1], render=render):
if tracker is not None:
tracker.StartTrackObject(robot, obj.GetName())
try:
if manip.simulated:
# Use the push planner code to simulate path execution.
# This simulates the pushing of the objects during
# execution of the trajectory.
planner.ExecutePlannedPath()
else:
# Execute the trajectory
robot.ExecuteTrajectory(traj)
# During execution, object pushes won't be simulated.
# In the OpenRAVE world, the robot will instead move
# through the objects and probably be in collision at
# the end. This call sets all the objects to their
# expected poses at the end of the trajectory.
# If execution was successful, this should resolve
# collisions.
planner.SetFinalObjectPoses()
finally:
if tracker is not None:
tracker.StopTrackObject(robot, obj.GetName())
return traj
def DetectBlocks(self, robot, table, blocks=None,timeout=10, **kw_args):
"""
Calls detector for blocks and places them on the table
@param robot: The robot instance using the detector
@param table: The kinbody for the table on which the blocks are placed
@blocks: List of blocks currently in the environment; if present, redetection not done
@return The list of blocks detected
"""
if blocks is not None and len(blocks) == 0:
# Add all blocks
env = robot.GetEnv()
# Get the pr-ordata package path
import prpy.util
block_path = os.path.join('objects', 'block.kinbody.xml')
detected_blocks = self.find_blocks(cloud_topic=self.point_cloud_topic)
# Place blocks on the table
from prpy.util import ComputeEnabledAABB
with prpy.rave.Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
z = table_aabb.pos()[2] + table_aabb.extents()[2] + table_z_offset #OFFSET SET AT TOP
for b in detected_blocks:
block = env.ReadKinBodyXMLFile(block_path)
for link in block.GetLinks():
for geometry in link.GetGeometries():
geometry.SetDiffuseColor(numpy.array([b.avg_color.r,b.avg_color.g,b.avg_color.b,b.avg_color.a]))
block_pose = numpy.array(quaternion_matrix([
b.pose.orientation.x,
b.pose.orientation.y,
b.pose.orientation.z,
b.pose.orientation.w]))
block_pose[0,3] = b.pose.position.x
block_pose[1,3] = b.pose.position.y
block_pose[2,3] = b.pose.position.z
self.listener.waitForTransform(
self.detection_frame,
self.destination_frame,
rospy.Time(0),
rospy.Duration(timeout))
frame_trans, frame_rot = self.listener.lookupTransform(
self.destination_frame,
self.detection_frame,
rospy.Time(0))
frame_offset = numpy.matrix(quaternion_matrix(frame_rot))
frame_offset[0,3] = frame_trans[0]
frame_offset[1,3] = frame_trans[1]
frame_offset[2,3] = frame_trans[2]
block_in_world = numpy.array(numpy.dot(frame_offset,block_pose))
#Snap block to table
block_in_world[2,3] = z
#To snap blocks to upright on table
block_matrix = block_in_world[0:3,0:3]
ax, ay, az = euler_from_matrix(block_matrix)
ax = 0
ay = 0
block_in_world_corrected = euler_matrix(ax,ay,az)
block_in_world[0:3,0:3] = block_in_world_corrected[0:3,0:3]
block.SetTransform(block_in_world)
#Set block name
block.SetName('block')
env.Add(block,anonymous=True)
blocks.append(block)
return blocks
def DetectBlocks(self, robot, table, blocks=None, timeout=10, **kw_args):
"""
Calls detector for blocks and places them on the table
@param robot: The robot instance using the detector
@param table: The kinbody for the table on which the blocks are placed
@blocks: List of blocks currently in the environment; if present, redetection not done
@return The list of blocks detected
"""
if blocks is not None and len(blocks) == 0:
# Add all blocks
env = robot.GetEnv()
# Get the pr-ordata package path
import prpy.util
block_path = os.path.join('objects', 'block.kinbody.xml')
detected_blocks = self.find_blocks(
cloud_topic=self.point_cloud_topic)
# Place blocks on the table
from prpy.util import ComputeEnabledAABB
with prpy.rave.Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
z = table_aabb.pos()[2] + table_aabb.extents(
)[2] + table_z_offset #OFFSET SET AT TOP
for b in detected_blocks:
block = env.ReadKinBodyXMLFile(block_path)
for link in block.GetLinks():
for geometry in link.GetGeometries():
geometry.SetDiffuseColor(
numpy.array([
b.avg_color.r, b.avg_color.g, b.avg_color.b,
b.avg_color.a
]))
block_pose = numpy.array(
quaternion_matrix([
b.pose.orientation.x, b.pose.orientation.y,
b.pose.orientation.z, b.pose.orientation.w
]))
block_pose[0, 3] = b.pose.position.x
block_pose[1, 3] = b.pose.position.y
block_pose[2, 3] = b.pose.position.z
self.listener.waitForTransform(self.detection_frame,
self.destination_frame,
rospy.Time(0),
rospy.Duration(timeout))
frame_trans, frame_rot = self.listener.lookupTransform(
self.destination_frame, self.detection_frame,
rospy.Time(0))
frame_offset = numpy.matrix(quaternion_matrix(frame_rot))
frame_offset[0, 3] = frame_trans[0]
frame_offset[1, 3] = frame_trans[1]
frame_offset[2, 3] = frame_trans[2]
block_in_world = numpy.array(
numpy.dot(frame_offset, block_pose))
#Snap block to table
block_in_world[2, 3] = z
#To snap blocks to upright on table
block_matrix = block_in_world[0:3, 0:3]
ax, ay, az = euler_from_matrix(block_matrix)
ax = 0
ay = 0
block_in_world_corrected = euler_matrix(ax, ay, az)
block_in_world[0:3, 0:3] = block_in_world_corrected[0:3, 0:3]
block.SetTransform(block_in_world)
#Set block name
block.SetName('block')
env.Add(block, anonymous=True)
blocks.append(block)
return blocks
env.Add(glass)
glass_transform = numpy.eye(4)
glass_transform[:3,3] = [0.6239455840637041, -0.4013916328109689, 0.75]
glass.SetTransform(glass_transform)
# Goal pose for the object
import numpy
goal_in_table = [0.15, 0., -0.03, 1.]
goal_in_world = numpy.dot(table.GetTransform(), goal_in_table)
goal_pose = goal_in_world[:2]
goal_radius = 0.1
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
table_height = 0.01 + table_aabb.pos()[2] + table_aabb.extents()[2]
pts = []
for a in numpy.arange(0, 2.*numpy.pi, 0.1):
pts.append([goal_pose[0] + goal_radius*numpy.sin(a),
goal_pose[1] + goal_radius*numpy.cos(a),
table_height])
h = env.drawlinestrip(points = numpy.array(pts), linewidth=5.0,
colors=numpy.array([0.0, 1., 0.]))
# Plan to the start configuration
try:
push_arm.SetActive()
start_pose = [ 4.49119545, -1.59899798, -0.6, 1.65274406, -1.7742985, -0.63854765, -1.23051631]
push_arm.PlanToConfiguration(start_pose, execute=True)