def choose(self, pm) : # Computing the possible action possible_actions = [transform_state(move) for move in pm] if np.random.uniform() < self.epsilon : self.action = possible_actions[np.random.randint(len(possible_actions))] else : if str(self.state) not in self.means.keys(): self.means[str(self.state)] = np.zeros(self.n_actions) if str(self.state) not in self.visits_number.keys(): self.visits_number[str(self.state)] = np.zeros(self.n_actions) if str(self.state) not in self.iteration_number.keys(): self.iteration_number[str(self.state)] = 1 visit_number = self.visits_number[str(self.state)] it_number = self.iteration_number[str(self.state)] ucb_values = self.means[str(self.state)] + np.sqrt(2. * (np.log(it_number / visit_number))) ucb_values_possible = [ucb_values[i] if i in possible_actions else -100 for i in xrange(len(ucb_values))] self.action = np.argmax(ucb_values_possible) # Needs to transform the action into the appropriate format return transform_inverse(self.action)
def choose(self, pm) : # Computing the possible action possible_actions = [transform_state(move) for move in pm] if np.random.uniform() < self.epsilon : self.action = possible_actions[np.random.randint(len(possible_actions))] else : # The Q values corresponding to the possible moves Q_values_possible = [self.Q_values[i] if i in possible_actions else -100 for i in xrange(len(self.Q_values))] self.action = np.argmax(Q_values_possible) # Needs to transform the action into the appropriate format return transform_inverse(self.action)
def __init__(self, delay=0, velocity_factor=0.1): # arm settings self.arm = moveit_commander.MoveGroupCommander("panda_arm") # transformations self.flange_to_tcp = [ 0.0, 0.0, 0.1035, 0.923879533, -0.382683432, 0.0, 0.0 ] self.tcp_to_flange = transform_inverse(self.flange_to_tcp).tolist() # set velocity scaling factor self.setMaxVelocityScalingFactor(velocity_factor) # wait for correct loading time.sleep(delay)
def get_inlier_model_and_scene_points_scene2model( neigh, model_points, scene_points, tf, outlier_max_distance=0.01, outlier_rejection_ratio=0.3): # Still returns results in world frame, but needs # the model tf to get the scene points in model # frame to do the closest point check. distances, indices = neigh.kneighbors( transform_points(transform_inverse(tf), scene_points[0:3, :]).T, return_distance=True) inlier_mask = np.logical_or( distances < outlier_max_distance, distances < distances.mean()*outlier_rejection_ratio)[:, 0] if inlier_mask.sum() == 0: inlier_mask += True model_pts_inlier = model_points[:, indices[inlier_mask]][:, :, 0] scene_pts_inlier = scene_points[:, inlier_mask] return model_pts_inlier, scene_pts_inlier
for instance_j, instance_config in enumerate(config["instances"]): new_rbt = RigidBodyTree() add_single_instance_to_rbt(new_rbt, config, instance_config, instance_j) new_rbt.compile() single_object_rbts.append(new_rbt) all_points = [] all_points_normals = [] all_points_labels = [] all_points_distances = [[] for i in range(len(config["instances"]))] for i, viewpoint in enumerate(camera_config["viewpoints"]): camera_tf = lookat(viewpoint["eye"], viewpoint["target"], viewpoint["up"]) camera_tf = camera_tf.dot(transform_inverse(depth_camera_pose)) camera_rpy = RollPitchYaw(RotationMatrix(camera_tf[0:3, 0:3])) q0 = np.zeros(6) q0[0:3] = camera_tf[0:3, 3] q0[3:6] = camera_rpy.vector() depth_image_name = "%09d_depth.png" % (i) depth_image = np.array( imageio.imread(os.path.join(args.dir, depth_image_name))) / 1000. depth_image_drake = Image[PixelType.kDepth32F](depth_image.shape[1], depth_image.shape[0]) depth_image_drake.mutable_data[:, :, 0] = depth_image[:, :] points = camera.ConvertDepthImageToPointCloud( depth_image_drake, camera.depth_camera_info()) good_points_mask = np.all(np.isfinite(points), axis=0) points = points[:, good_points_mask] points = np.vstack([points, np.ones([1, points.shape[1]])])
def test_tf(arm): if isinstance(arm, PandaArm): import tf2_ros from geometry_msgs.msg import TransformStamped from utils import quaternion_equals # BROADCAST BASE-TO-TCP TRANSFORMATION # broadcaster = tf2_ros.StaticTransformBroadcaster() # static_transformStamped = TransformStamped() # static_transformStamped.header.stamp = rospy.Time.now() # static_transformStamped.header.frame_id = "panda_link8" # static_transformStamped.child_frame_id = "tcp" # static_transformStamped.transform.translation.x = 0.0 # static_transformStamped.transform.translation.y = 0.0 # static_transformStamped.transform.translation.z = 0.1035 # static_transformStamped.transform.rotation.x = 0.923879533 # static_transformStamped.transform.rotation.y = -0.382683432 # static_transformStamped.transform.rotation.z = 0.0 # static_transformStamped.transform.rotation.w = 0.0 # broadcaster.sendTransform(static_transformStamped) # CREATE TF2 LISTENER tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) time.sleep(2) # TESTO0 # generate current pose with TF2 world_to_tcp_listened = tf_buffer.lookup_transform( "world", "tcp", rospy.Time()) world_to_tcp_tf2 = list() world_to_tcp_tf2.append(world_to_tcp_listened.transform.translation.x) world_to_tcp_tf2.append(world_to_tcp_listened.transform.translation.y) world_to_tcp_tf2.append(world_to_tcp_listened.transform.translation.z) world_to_tcp_tf2.append(world_to_tcp_listened.transform.rotation.x) world_to_tcp_tf2.append(world_to_tcp_listened.transform.rotation.y) world_to_tcp_tf2.append(world_to_tcp_listened.transform.rotation.z) world_to_tcp_tf2.append(world_to_tcp_listened.transform.rotation.w) print("current pose with TF2:") print(world_to_tcp_tf2) # TEST1 # generate transformations with TF2 print("WITH TF2: ") flange_to_tcp_listened = tf_buffer.lookup_transform( "panda_link8", "tcp", rospy.Time()) flange_to_tcp_tf2 = list() flange_to_tcp_tf2.append( flange_to_tcp_listened.transform.translation.x) flange_to_tcp_tf2.append( flange_to_tcp_listened.transform.translation.y) flange_to_tcp_tf2.append( flange_to_tcp_listened.transform.translation.z) flange_to_tcp_tf2.append(flange_to_tcp_listened.transform.rotation.x) flange_to_tcp_tf2.append(flange_to_tcp_listened.transform.rotation.y) flange_to_tcp_tf2.append(flange_to_tcp_listened.transform.rotation.z) flange_to_tcp_tf2.append(flange_to_tcp_listened.transform.rotation.w) print("flange_to_tcp: {}".format(flange_to_tcp_tf2)) tcp_to_flange_listened = tf_buffer.lookup_transform( "tcp", "panda_link8", rospy.Time()) tcp_to_flange_tf2 = list() tcp_to_flange_tf2.append( tcp_to_flange_listened.transform.translation.x) tcp_to_flange_tf2.append( tcp_to_flange_listened.transform.translation.y) tcp_to_flange_tf2.append( tcp_to_flange_listened.transform.translation.z) tcp_to_flange_tf2.append(tcp_to_flange_listened.transform.rotation.x) tcp_to_flange_tf2.append(tcp_to_flange_listened.transform.rotation.y) tcp_to_flange_tf2.append(tcp_to_flange_listened.transform.rotation.z) tcp_to_flange_tf2.append(tcp_to_flange_listened.transform.rotation.w) print("tcp_to_flange: {}\n".format(tcp_to_flange_tf2)) # generete transformations with my UTILS file print("WITH UTILS") flange_to_tcp_utils_fake = [ 0.0, 0.0, 0.1035, 0.923879533, -0.382683432, 0.0, 0.0 ] print("flange_to_tcp: {}".format(flange_to_tcp_utils_fake)) tcp_to_flange_utils = transform_inverse(flange_to_tcp_utils_fake) print("tcp_to_flange: {}".format(tcp_to_flange_utils.tolist())) flange_to_tcp_utils = transform_inverse(tcp_to_flange_utils) print("flange_to_tcp_utils: {}\n".format(flange_to_tcp_utils.tolist())) # checks tf2 vs utils print("Check TF2 with UTILS: ({}, {})".format( \ quaternion_equals(flange_to_tcp_tf2[3:], flange_to_tcp_utils[3:]), \ quaternion_equals(tcp_to_flange_tf2[3:], tcp_to_flange_utils[3:]))) print("Check UTILS: fake vs inverse(inverse(fake)): {}\n".format( \ quaternion_equals(flange_to_tcp_utils_fake[3:], flange_to_tcp_utils[3:]))) # checks consistence with PandaArm print("Check ARM. with UTILS: ({}, {})".format( \ quaternion_equals(arm.flange_to_tcp[3:], flange_to_tcp_utils[3:]), \ quaternion_equals(arm.tcp_to_flange[3:], tcp_to_flange_utils[3:]))) # TEST2 print("----------------------------") world_to_tcp_fake = [0.3, 0, 0.5, 0, 0, 0, 1] print("world_to_tcp: {}".format(world_to_tcp_fake)) world_to_flange_utils = transform(world_to_tcp_fake, tcp_to_flange_utils) print("world_to_flange_utils: {}".format( world_to_flange_utils.tolist())) world_to_flange_arm_method = arm.getFlangeFromTCP(world_to_tcp_fake) print("world_to_flange from getFlangeFromTCP(): {}".format( world_to_flange_arm_method)) print("Check Utils with getFlangeFromTCP() {}\n".format( \ quaternion_equals(world_to_flange_utils[3:], world_to_flange_arm_method[3:]))) world_to_tcp_utils = transform(world_to_flange_utils, flange_to_tcp_utils) print("world_to_tcp_my: {}".format(world_to_tcp_utils.tolist())) print("Check world_to_tcp_fake with utils: {}\n".format( \ quaternion_equals(world_to_tcp_fake[3:], world_to_tcp_utils[3:])))