def __init__(self, rave_only=False):
# set up openrave
self.env = rave.Environment()
self.env.StopSimulation()
self.env.Load("robots/pr2-beta-static.zae") # todo: use up-to-date urdf
self.robot = self.env.GetRobots()[0]
# set up arm ik solvers (discretization level, etc.)
for manip in [self.robot.GetManipulator("leftarm"), self.robot.GetManipulator("rightarm")]:
self.robot.SetActiveManipulator(manip)
ikmodel = rave.databases.inversekinematics.InverseKinematicsModel(
self.robot,
iktype=rave.IkParameterization.Type.Transform6D,
freeindices=[manip.GetArmIndices()[2]],
forceikfast=True
)
if not ikmodel.load():
ikmodel.autogenerate()
#if not ikmodel.setrobot(freeinc=[0.1]):
# raise RuntimeError('failed to load ik')
if not rave_only:
self.joint_listener = TopicListener("/joint_states", sm.JointState)
self.tf_listener = ros_utils.get_tf_listener()
# rave to ros conversions
joint_msg = self.get_last_joint_message()
ros_names = joint_msg.name
inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
self.update_rave()
self.larm = Arm(self, "l")
self.rarm = Arm(self, "r")
self.lgrip = Gripper(self, "l")
self.rgrip = Gripper(self, "r")
self.head = Head(self)
self.torso = Torso(self)
self.base = Base(self)
# make the joint limits match the PR2 soft limits
low_limits, high_limits = self.robot.GetDOFLimits()
rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(), rarm_low_limits, rarm_high_limits):
if low is not None and high is not None:
low_limits[rarm_index] = low
high_limits[rarm_index] = high
larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(), larm_low_limits, larm_high_limits):
if low is not None and high is not None:
low_limits[larm_index] = low
high_limits[larm_index] = high
self.robot.SetDOFLimits(low_limits, high_limits)
rospy.on_shutdown(self.stop_all)
def read_from_ros(cloud_topic, frame='base_footprint'):
'''format: ros:/my/topic/points(:frame)'''
import rospy
from brett2 import ros_utils
import sensor_msgs
import time
if rospy.get_name() == '/unnamed':
rospy.init_node('cloud_reader', disable_signals=True)
msg = rospy.wait_for_message(cloud_topic, sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1, 3)
tf_listener = ros_utils.get_tf_listener()
return ros_utils.transform_points(xyz, tf_listener, frame, msg.header.frame_id)
def main():
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('--dataset', default='overhand_knot', help='name of dataset')
parser.add_argument('--method', choices=['geodesic_dist', 'shape_context', 'geodesic_dist+shape_context'], default='geodesic_dist', help='matching algorithm')
parser.add_argument('--input_mode', choices=['from_dataset', 'kinect', 'file'], default='kinect', help='input cloud acquisition method')
parser.add_argument('--cloud_topic', default='/preprocessor/kinect1/points', help='ros topic for kinect input mode')
parser.add_argument('--input_file', help='input file for --input_mode=file')
parser.add_argument('--show_shape_context', action='store_true')
args = parser.parse_args()
dataset = recognition.DataSet.LoadFromTaskDemos(args.dataset)
# read input to match
input_xyz = None
if args.input_mode == 'from_dataset':
key = select_from_list(sorted(dataset.keys()))
input_xyz = np.squeeze(np.asarray(dataset[key]['cloud_xyz']))
elif args.input_mode == 'kinect':
import rospy
from brett2 import ros_utils
import sensor_msgs
import time
if rospy.get_name() == '/unnamed':
rospy.init_node('matcher', disable_signals=True)
msg = rospy.wait_for_message(args.cloud_topic, sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1, 3)
tf_listener = ros_utils.get_tf_listener()
time.sleep(1) # so tf works
input_xyz = ros_utils.transform_points(xyz, tf_listener, "ground", msg.header.frame_id)
elif args.input_mode == 'file':
input_xyz = np.load(args.input_file)['cloud_xyz']
else:
raise NotImplementedError
# create the matcher
matcher = None
if args.method == 'geodesic_dist':
matcher = recognition.GeodesicDistMatcher(dataset)
elif args.method == 'shape_context':
matcher = recognition.ShapeContextMatcher(dataset)
elif args.method == 'geodesic_dist+shape_context':
matcher = recognition.CombinedNNMatcher(dataset, [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1])
else:
raise NotImplementedError
# do the matching
best_match_name, best_match_cost = matcher.match(input_xyz)
print 'Best match name:', best_match_name, 'with cost:', best_match_cost
draw_comparison(left_cloud=input_xyz, right_cloud=dataset[best_match_name], show_shape_context=args.show_shape_context)
def read_from_ros(cloud_topic, frame='ground'):
'''format: ros:/my/topic/points(:frame)'''
import rospy
from brett2 import ros_utils
import sensor_msgs
import time
if rospy.get_name() == '/unnamed':
rospy.init_node('cloud_reader', disable_signals=True)
msg = rospy.wait_for_message(cloud_topic, sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1, 3)
tf_listener = ros_utils.get_tf_listener()
return ros_utils.transform_points(xyz, tf_listener, frame,
msg.header.frame_id)
def __init__(self, rave_only=False):
# set up openrave
self.env = rave.Environment()
self.env.StopSimulation()
self.env.Load("robots/pr2-beta-static.zae") # todo: use up-to-date urdf
self.robot = self.env.GetRobots()[0]
if not rave_only:
self.joint_listener = TopicListener("/joint_states", sm.JointState)
self.tf_listener = ros_utils.get_tf_listener()
# rave to ros conversions
joint_msg = self.get_last_joint_message()
ros_names = joint_msg.name
inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
self.update_rave()
self.larm = Arm(self, "l")
self.rarm = Arm(self, "r")
self.lgrip = Gripper(self, "l")
self.rgrip = Gripper(self, "r")
self.head = Head(self)
self.torso = Torso(self)
self.base = Base(self)
# make the joint limits match the PR2 soft limits
low_limits, high_limits = self.robot.GetDOFLimits()
rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(), rarm_low_limits, rarm_high_limits):
if low is not None and high is not None:
low_limits[rarm_index] = low
high_limits[rarm_index] = high
larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(), larm_low_limits, larm_high_limits):
if low is not None and high is not None:
low_limits[larm_index] = low
high_limits[larm_index] = high
self.robot.SetDOFLimits(low_limits, high_limits)
rospy.on_shutdown(self.stop_all)
def setup(): if Globals.rviz is None: Globals.rviz = ros_utils.RvizWrapper.create() if Globals.tf_listener is None: Globals.tf_listener = ros_utils.get_tf_listener()
def main():
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('--dataset',
default='overhand_knot',
help='name of dataset')
parser.add_argument('--method',
choices=[
'geodesic_dist', 'shape_context',
'geodesic_dist+shape_context'
],
default='geodesic_dist',
help='matching algorithm')
parser.add_argument('--input_mode',
choices=['from_dataset', 'kinect', 'file'],
default='kinect',
help='input cloud acquisition method')
parser.add_argument('--cloud_topic',
default='/preprocessor/kinect1/points',
help='ros topic for kinect input mode')
parser.add_argument('--input_file',
help='input file for --input_mode=file')
parser.add_argument('--show_shape_context', action='store_true')
args = parser.parse_args()
dataset = recognition.DataSet.LoadFromTaskDemos(args.dataset)
# read input to match
input_xyz = None
if args.input_mode == 'from_dataset':
key = select_from_list(sorted(dataset.keys()))
input_xyz = np.squeeze(np.asarray(dataset[key]['cloud_xyz']))
elif args.input_mode == 'kinect':
import rospy
from brett2 import ros_utils
import sensor_msgs
import time
if rospy.get_name() == '/unnamed':
rospy.init_node('matcher', disable_signals=True)
msg = rospy.wait_for_message(args.cloud_topic,
sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1, 3)
tf_listener = ros_utils.get_tf_listener()
time.sleep(1) # so tf works
input_xyz = ros_utils.transform_points(xyz, tf_listener, "ground",
msg.header.frame_id)
elif args.input_mode == 'file':
input_xyz = np.load(args.input_file)['cloud_xyz']
else:
raise NotImplementedError
# create the matcher
matcher = None
if args.method == 'geodesic_dist':
matcher = recognition.GeodesicDistMatcher(dataset)
elif args.method == 'shape_context':
matcher = recognition.ShapeContextMatcher(dataset)
elif args.method == 'geodesic_dist+shape_context':
matcher = recognition.CombinedNNMatcher(
dataset,
[recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher],
[1, 0.1])
else:
raise NotImplementedError
# do the matching
best_match_name, best_match_cost = matcher.match(input_xyz)
print 'Best match name:', best_match_name, 'with cost:', best_match_cost
draw_comparison(left_cloud=input_xyz,
right_cloud=dataset[best_match_name],
show_shape_context=args.show_shape_context)
def grip_to_world_transform_tf(point):
return ru.transform_points(np.array([point]), ru.get_tf_listener(), gripper_frame_name, "base_footprint")[0]
def to_gripper_frame_tf_listener(pc, gripper_frame_name):
return ru.transform_points(pc, ru.get_tf_listener(), gripper_frame_name, "base_footprint")
def setup():
if Globals.rviz is None: Globals.rviz = ros_utils.RvizWrapper.create()
if Globals.tf_listener is None:
Globals.tf_listener = ros_utils.get_tf_listener()
def grip_to_world_transform_tf(point):
return ru.transform_points(np.array([point]), ru.get_tf_listener(), gripper_frame_name, "base_footprint")[0]
