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:])))