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]