def randomPoseScore(bin_num, withScore):
typical_poses = \
[[1.58220350742, 0.287826299667, 1.12025654316, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 1.12202310562, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 1.12350583076, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.901469767094, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.9014697670942, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.901469767094, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023]]
obj_pose = typical_poses[bin_num]
obj_pose[0] += random.uniform(-0.1, 0.1)
obj_pose[1] += random.uniform(-0.1, 0.1)
obj_pose[2] += random.uniform(-0.1, 0.1)
obj_pose[3:7] = tfm.random_quaternion(rand=None).tolist()
thres = 0
if withScore:
if random.uniform(0,1) >= thres:
return (obj_pose, random.uniform(0.1, 3))
else:
return None, None
else:
if random.uniform(0,1) >= thres:
return obj_pose
else:
return None
def randomPoseScore(bin_num, withScore):
typical_poses = \
[[1.58220350742, 0.287826299667, 1.12025654316, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 1.12202310562, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 1.12350583076, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.901469767094, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.9014697670942, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.901469767094, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.658816933632, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58220350742, 0.287826299667, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58204042912, -0.0443051755428, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023],
[1.58190357685, -0.323061853647, 0.434227764606, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023]]
obj_pose = typical_poses[bin_num]
obj_pose[0] += random.uniform(-0.1, 0.1)
obj_pose[1] += random.uniform(-0.1, 0.1)
obj_pose[2] += random.uniform(-0.1, 0.1)
obj_pose[3:7] = tfm.random_quaternion(rand=None).tolist()
thres = 0
if withScore:
if random.uniform(0, 1) >= thres:
return (obj_pose, random.uniform(0.1, 3))
else:
return None, None
else:
if random.uniform(0, 1) >= thres:
return obj_pose
else:
return None
def test_create_msg(self): for _ in xrange(50): q = tft.random_quaternion() q_msg = self.getMsg(q) tb = tb_angles(q_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list())) self.assertMsgEqual(q_msg, tb.msg) self.assertMsgEqual(q_msg, tb.to_quaternion_msg()) qs_msg = geometry_msgs.msg.QuaternionStamped() qs_msg.quaternion = q_msg tb = tb_angles(qs_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list())) pose_msg = geometry_msgs.msg.Pose() pose_msg.orientation = q_msg tb = tb_angles(pose_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list())) pose_stamped_msg = geometry_msgs.msg.PoseStamped() pose_stamped_msg.pose.orientation = q_msg tb = tb_angles(pose_stamped_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list())) tf_msg = geometry_msgs.msg.Transform() tf_msg.rotation = q_msg tb = tb_angles(tf_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list())) tf_stamped_msg = geometry_msgs.msg.TransformStamped() tf_stamped_msg.transform.rotation = q_msg tb = tb_angles(tf_stamped_msg) self.assertTrue(np.allclose(q, tb.quaternion) or np.allclose(-q, tb.quaternion),msg='%s and %s not close!' % (list(q),tb.to_quaternion_list()))
def goalCB(self, data):
state = State()
axis_z=[0,0,1]
#Hopf fibration approach
thrust=np.array([data.a.x, data.a.y, data.a.z + 9.81]);
thrust_normalized=thrust/np.linalg.norm(thrust);
a=thrust_normalized[0];
b=thrust_normalized[1];
c=thrust_normalized[2];
qabc = random_quaternion();
tmp=(1/math.sqrt(2*(1+c)));
#From http://docs.ros.org/en/jade/api/tf/html/python/transformations.html, the documentation says
#"Quaternions ix+jy+kz+w are represented as [x, y, z, w]."
qabc[3] = tmp*(1+c) #w
qabc[0] = tmp*(-b) #x
qabc[1] = tmp*(a) #y
qabc[2] = 0 #z
qpsi = random_quaternion();
qpsi[3] = math.cos(data.psi/2.0); #w
qpsi[0] = 0; #x
qpsi[1] = 0; #y
qpsi[2] = math.sin(data.psi/2.0); #z
w_q_b=quaternion_multiply(qabc,qpsi)
# accel=[data.a.x, data.a.y, data.a.z + 9.81];
self.state.header.frame_id="world"
self.state.pos=data.p
self.state.vel=data.v
self.state.quat.w=w_q_b[3] #w
self.state.quat.x=w_q_b[0] #x
self.state.quat.y=w_q_b[1] #y
self.state.quat.z=w_q_b[2] #z
self.pubState.publish(self.state)
if(self.publish_marker_drone):
self.pubMarkerDrone.publish(self.getDroneMarker());
def cb(self, ps):
self.ps.pose.orientation = Quaternion(*tft.random_quaternion())
print ps
rospy.sleep(0.5)
self.recd_pub.publish(ps)
#trans_pose = pose_relative_trans(ps, 0.5, 0.5, 0.2)
#self.trans_pub.publish(trans_pose)
#rospy.loginfo("Pose Utils Test: Pose Translated: \n\r %s" %trans_pose)
ps_rot = pose_relative_rot(ps, 90, 30, 45)
self.rot_pub.publish(ps_rot)
rospy.loginfo("Pose Utils Test: Pose Rotated: \n\r %s" % ps_rot)
def cb(self, ps):
self.ps.pose.orientation = Quaternion(*tft.random_quaternion())
print ps
rospy.sleep(0.5)
self.recd_pub.publish(ps)
#trans_pose = pose_relative_trans(ps, 0.5, 0.5, 0.2)
#self.trans_pub.publish(trans_pose)
#rospy.loginfo("Pose Utils Test: Pose Translated: \n\r %s" %trans_pose)
ps_rot = pose_relative_rot(ps, 90, 30 , 45)
self.rot_pub.publish(ps_rot)
rospy.loginfo("Pose Utils Test: Pose Rotated: \n\r %s" %ps_rot)
def test_create_msg(self):
for _ in xrange(50):
q = tft.random_quaternion()
q_msg = self.getMsg(q)
tb = tb_angles(q_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
self.assertMsgEqual(q_msg, tb.msg)
self.assertMsgEqual(q_msg, tb.to_quaternion_msg())
qs_msg = geometry_msgs.msg.QuaternionStamped()
qs_msg.quaternion = q_msg
tb = tb_angles(qs_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
pose_msg = geometry_msgs.msg.Pose()
pose_msg.orientation = q_msg
tb = tb_angles(pose_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
pose_stamped_msg = geometry_msgs.msg.PoseStamped()
pose_stamped_msg.pose.orientation = q_msg
tb = tb_angles(pose_stamped_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
tf_msg = geometry_msgs.msg.Transform()
tf_msg.rotation = q_msg
tb = tb_angles(tf_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
tf_stamped_msg = geometry_msgs.msg.TransformStamped()
tf_stamped_msg.transform.rotation = q_msg
tb = tb_angles(tf_stamped_msg)
self.assertTrue(np.allclose(q, tb.quaternion)
or np.allclose(-q, tb.quaternion),
msg='%s and %s not close!' %
(list(q), tb.to_quaternion_list()))
def test_triad():
q = transformations.random_quaternion()
a = np.random.standard_normal(3)
b = np.random.standard_normal(3)
m = transformations.quaternion_matrix(q)[:3, :3]
q_ = triad((m.dot(a), m.dot(b)), (a, b))
assert np.linalg.norm(quat_to_rotvec(
transformations.quaternion_multiply(
q,
transformations.quaternion_inverse(q_),
)
)) < 1e-6
def test_triad():
q = transformations.random_quaternion()
a = np.random.standard_normal(3)
b = np.random.standard_normal(3)
m = transformations.quaternion_matrix(q)[:3, :3]
q_ = triad((m.dot(a), m.dot(b)), (a, b))
assert np.linalg.norm(quat_to_rotvec(
transformations.quaternion_multiply(
q,
transformations.quaternion_inverse(q_),
)
)) < 1e-6
def random_landmark(self):
landmark = LandmarkEntry()
landmark.translation_weight = self.options.translation_weight
landmark.rotation_weight = self.options.rotation_weight
landmark.id = self.landmark_id_sampler.sample_id()
if landmark.id in self._sampled_ids:
if not self.options.allow_duplicate_ids:
rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id))
return None
else:
rospy.logwarn("Duplicate ID: {}".format(landmark.id))
self._sampled_ids.append(landmark.id)
vector = transformations.random_vector(3) * self.options.max_distance
landmark.tracking_from_landmark_transform.position.x = vector[0]
landmark.tracking_from_landmark_transform.position.y = vector[1]
landmark.tracking_from_landmark_transform.position.z = vector[2]
quaternion = transformations.random_quaternion()
landmark.tracking_from_landmark_transform.orientation.x = quaternion[0]
landmark.tracking_from_landmark_transform.orientation.y = quaternion[1]
landmark.tracking_from_landmark_transform.orientation.z = quaternion[2]
landmark.tracking_from_landmark_transform.orientation.w = quaternion[3]
return landmark
def random_landmark(self):
landmark = LandmarkEntry()
landmark.translation_weight = self.options.translation_weight
landmark.rotation_weight = self.options.rotation_weight
landmark.id = self.landmark_id_sampler.sample_id()
if landmark.id in self._sampled_ids:
if not self.options.allow_duplicate_ids:
rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id))
return None
else:
rospy.logwarn("Duplicate ID: {}".format(landmark.id))
self._sampled_ids.append(landmark.id)
vector = transformations.random_vector(3) * self.options.max_distance
landmark.tracking_from_landmark_transform.position.x = vector[0]
landmark.tracking_from_landmark_transform.position.y = vector[1]
landmark.tracking_from_landmark_transform.position.z = vector[2]
quaternion = transformations.random_quaternion()
landmark.tracking_from_landmark_transform.orientation.x = quaternion[0]
landmark.tracking_from_landmark_transform.orientation.y = quaternion[1]
landmark.tracking_from_landmark_transform.orientation.z = quaternion[2]
landmark.tracking_from_landmark_transform.orientation.w = quaternion[3]
return landmark
def random_pose(_min, _max):
''' Gives a random pose in the xyz range `_min` to `_max` '''
pos = np.random.uniform(low=_min, high=_max, size=3)
quat = random_quaternion()
return numpy_quat_pair_to_pose(pos, quat)
def random_pose(_min, _max):
''' Gives a random pose in the xyz range `_min` to `_max` '''
pos = np.random.uniform(low=_min, high=_max, size=3)
quat = random_quaternion()
return numpy_quat_pair_to_pose(pos, quat)
def create_complete_object_description(): point2dmodel = Point2DModel() point2dmodel.type = '2dpoint' point2dmodel.geometry.x = -0.5 point2dmodel.geometry.y = 0.0 point2dmodel.geometry.z = 0.0 pose2dmodel = Pose2DModel() pose2dmodel.type = '2dpose' pose2dmodel.pose = ROSPose2D() pose2dmodel.pose.x = 0.5 pose2dmodel.pose.y = 1.5 pose2dmodel.pose.theta = 1.5 point0 = ROSPoint32(0, 0, 0) point1 = ROSPoint32(1, 0, 0) point2 = ROSPoint32(1, 1, 0) point3 = ROSPoint32(0, 1, 0) point4 = ROSPoint32(-1, 0, 0.5) point5 = ROSPoint32(-1, 1, 0.5) point6 = ROSPoint32(1, 1, 0) point7 = ROSPoint32(2, 1, 0) point8 = ROSPoint32(2, 2, 0) point9 = ROSPoint32(1, 2, 0) point10 = ROSPoint32(-2, 1, 0) point11 = ROSPoint32(-2, 2, 0) polygon0 = ROSPolygon() polygon0.points.append(point0) polygon0.points.append(point1) polygon0.points.append(point2) polygon0.points.append(point3) polygon1 = ROSPolygon() polygon1.points.append(point4) polygon1.points.append(point0) polygon1.points.append(point3) polygon1.points.append(point5) polygon2 = ROSPolygon() polygon2.points.append(point6) polygon2.points.append(point7) polygon2.points.append(point8) polygon2.points.append(point9) polygon3 = ROSPolygon() polygon3.points.append(point10) polygon3.points.append(point6) polygon3.points.append(point9) polygon3.points.append(point11) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = '2dpolygon' polygon2dmodel.pose = ROSPose() polygon2dmodel.geometry = polygon0 point3dmodel = Point3DModel() point3dmodel.type = '3dpoint' point3dmodel.geometry.x = 2.0 point3dmodel.geometry.y = 2.0 point3dmodel.geometry.z = 1.0 pose3dmodel = Pose3DModel() pose3dmodel.type = '3dpose' pose3dmodel.pose = ROSPose() pose3dmodel.pose.position.x = -0.5 pose3dmodel.pose.position.y = -0.5 pose3dmodel.pose.position.z = 0.5 rand_quat = random_quaternion() pose3dmodel.pose.orientation.x = rand_quat[0] pose3dmodel.pose.orientation.y = rand_quat[1] pose3dmodel.pose.orientation.z = rand_quat[2] pose3dmodel.pose.orientation.w = rand_quat[3] polygon3dmodel = Polygon3DModel() polygon3dmodel.type = '3dpolygon' polygon3dmodel.pose = ROSPose() polygon3dmodel.pose.position.x = 2.0 polygon3dmodel.geometry = polygon1 tri0 = ROSMeshTriangle() tri1 = ROSMeshTriangle() tri0.vertex_indices[0] = 0 tri0.vertex_indices[1] = 1 tri0.vertex_indices[2] = 2 tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 2 tri1.vertex_indices[2] = 3 tpoint0 = ROSPoint( 0.0, 0.0, 0.0) tpoint1 = ROSPoint( 1.0, 0.0, 1.0) tpoint2 = ROSPoint( 1.0, 1.0, 0.0) tpoint3 = ROSPoint( 0.0, 1.0, 1.0) tpoint4 = ROSPoint(-1.0, 0.0, 0.0) tpoint5 = ROSPoint(-1.0, 1.0, 1.0) trianglemesh3dmodel = TriangleMesh3DModel() trianglemesh3dmodel.type = '3dtrianglemesh' trianglemesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() trianglemesh3dmodel.pose.position.x = 1.0 trianglemesh3dmodel.pose.position.y = -2.0 trianglemesh3dmodel.pose.position.z = 0.25 trianglemesh3dmodel.pose.orientation.x = 0.0 trianglemesh3dmodel.pose.orientation.y = 0.0 trianglemesh3dmodel.pose.orientation.z = 0.0 trianglemesh3dmodel.pose.orientation.w = 0.0 trianglemesh3dmodel.geometry.vertices.append(tpoint0) trianglemesh3dmodel.geometry.vertices.append(tpoint1) trianglemesh3dmodel.geometry.vertices.append(tpoint2) trianglemesh3dmodel.geometry.vertices.append(tpoint3) trianglemesh3dmodel.geometry.triangles.append(tri0) trianglemesh3dmodel.geometry.triangles.append(tri1) polygonmesh3dmodel = PolygonMesh3DModel() polygonmesh3dmodel.type = '3dpolygonmesh' polygonmesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() polygonmesh3dmodel.pose.position.x = 0.5 polygonmesh3dmodel.pose.position.y = 1.0 polygonmesh3dmodel.pose.position.z = 0.0 polygonmesh3dmodel.pose.orientation.x = 0.0 polygonmesh3dmodel.pose.orientation.y = 0.0 polygonmesh3dmodel.pose.orientation.z = 0.0 polygonmesh3dmodel.pose.orientation.w = 0.0 polygonmesh3dmodel.geometry.polygons.append(polygon2) polygonmesh3dmodel.geometry.polygons.append(polygon3) desc_ros = ROSObjectDescription() desc_ros.type = "Geometry" desc_ros.point2d_models.append(point2dmodel) desc_ros.pose2d_models.append(pose2dmodel) desc_ros.polygon2d_models.append(polygon2dmodel) desc_ros.point3d_models.append(point3dmodel) desc_ros.pose3d_models.append(pose3dmodel) desc_ros.polygon3d_models.append(polygon3dmodel) desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel) desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel) return desc_ros
def make_random():
return Quaternion(tftf.random_quaternion())
import roslib; roslib.load_manifest('pr2_python')
import numpy as np
import rospy
from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped
from tf import transformations
from pr2_python.arm_controller import ArmController
from pr2_python.exceptions import ArmNavError
arm_name = 'right_arm'
rospy.init_node('test_arm_control', anonymous=True)
ac = ArmController(arm_name)
while not rospy.is_shutdown():
rot = transformations.random_quaternion()
#trans = np.random.uniform(-0.5, 0.5, 3) + np.array((
trans = np.array((0.65, 0.0, 0.0)) + np.random.uniform(-0.6, 0.6, 3)
rot = np.array((0.0, 0.0, 0.0, 1.0))
print 'Moving to %s, %s' % (str(rot), str(trans))
goal_pose = PoseStamped()
goal_pose.pose.position = Point(*trans)
goal_pose.pose.orientation = Quaternion(*rot)
goal_pose.header.frame_id = '/torso_lift_link'
try:
ac.move_to_goal(goal_pose, try_hard=True)
print ac.get_exceptions()
except ArmNavError as e:
print 'Arm nav error: %s' % str(e)
rospy.sleep(0.1)
def test_object_instance_insertion(): point2dmodel = Point2DModel() point2dmodel.type = '2dpoint' point2dmodel.geometry.x = 17.0 point2dmodel.geometry.y = 4.0 point2dmodel.geometry.z = 11.0 pose2dmodel = Pose2DModel() pose2dmodel.type = '2dpose' pose2dmodel.pose = ROSPose2D() pose2dmodel.pose.x = 0.5 pose2dmodel.pose.y = 1.5 pose2dmodel.pose.theta = 0.5 point0 = ROSPoint32(0, 0, 0) point1 = ROSPoint32(1, 0, 1) point2 = ROSPoint32(1, 1, 0) point3 = ROSPoint32(0, 1, 1) point4 = ROSPoint32(-1, 0, 1) point5 = ROSPoint32(-1, 1, 1) polygon0 = ROSPolygon() polygon0.points.append(point0) polygon0.points.append(point1) polygon0.points.append(point2) polygon0.points.append(point3) polygon1 = ROSPolygon() polygon1.points.append(point4) polygon1.points.append(point0) polygon1.points.append(point3) polygon1.points.append(point5) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = '2dpolygon' polygon2dmodel.pose = ROSPose() polygon2dmodel.geometry = polygon0 point3dmodel = Point3DModel() point3dmodel.type = '3dpoint' point3dmodel.geometry.x = 19.0 point3dmodel.geometry.y = 66.0 point3dmodel.geometry.z = 12.0 pose3dmodel = Pose3DModel() pose3dmodel.type = '3dpose' pose3dmodel.pose = ROSPose() pose3dmodel.pose.position.x = 15.0 polygon3dmodel = Polygon3DModel() polygon3dmodel.type = '3dpolygon' polygon3dmodel.pose = ROSPose() polygon3dmodel.pose.position.x = 6.0 polygon3dmodel.geometry = polygon1 tri0 = ROSMeshTriangle() tri1 = ROSMeshTriangle() tri0.vertex_indices[0] = 0 tri0.vertex_indices[1] = 1 tri0.vertex_indices[2] = 2 tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 2 tri1.vertex_indices[2] = 3 trianglemesh3dmodel = TriangleMesh3DModel() trianglemesh3dmodel.type = '3dtrianglemesh' trianglemesh3dmodel.pose = ROSPose() trianglemesh3dmodel.geometry.vertices.append(point0) trianglemesh3dmodel.geometry.vertices.append(point1) trianglemesh3dmodel.geometry.vertices.append(point2) trianglemesh3dmodel.geometry.vertices.append(point3) trianglemesh3dmodel.geometry.triangles.append(tri0) trianglemesh3dmodel.geometry.triangles.append(tri1) polygonmesh3dmodel = PolygonMesh3DModel() polygonmesh3dmodel.type = '3dpolygonmesh' polygonmesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() polygonmesh3dmodel.pose.orientation.x = rand_quat[0] polygonmesh3dmodel.pose.orientation.y = rand_quat[1] polygonmesh3dmodel.pose.orientation.z = rand_quat[2] polygonmesh3dmodel.pose.orientation.w = rand_quat[3] polygonmesh3dmodel.geometry.polygons.append(polygon0) polygonmesh3dmodel.geometry.polygons.append(polygon1) desc_ros = ROSObjectDescription() desc_ros.type = "test_description" desc_ros.point2d_models.append(point2dmodel) desc_ros.pose2d_models.append(pose2dmodel) desc_ros.polygon2d_models.append(polygon2dmodel) desc_ros.point3d_models.append(point3dmodel) desc_ros.pose3d_models.append(pose3dmodel) desc_ros.polygon3d_models.append(polygon3dmodel) desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel) desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel) pose = ROSPoseStamped() pose.header.frame_id = "ref1" pose.pose.position.x = "0.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" pose.pose.orientation.x = "0.0" pose.pose.orientation.y = "0.0" pose.pose.orientation.z = "0.0" pose.pose.orientation.w = "1.0" inst_ros = ROSObjectInstance() inst_ros.alias = "mr_objecto" inst_ros.pose = pose inst_ros.description = desc_ros # create instance from ROS inst_db0 = ObjectInstance() inst_db0.fromROS(inst_ros) db().add(inst_db0) # create instance from existing model via id inst_db1 = ObjectInstance() inst_db1.object_description_id = 1 inst_db1.alias = "mrs_objecto" inst_db1.pose = GlobalPose() pose.pose.position.x = "1.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" inst_db1.pose.pose = fromROSPose(pose.pose) inst_db1.pose.ref_system = 'origin' #db().add(inst_db1) db().commit()
import tf.transformations as tf
test_enabled = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
# test_enabled = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]
num_test = 50
tot_test = 0
tot_successess = 0
# quaternion_from_euler and euler_from_quaternion
if test_enabled[0]:
print("Test Conversions:")
quaternion_successes = 0
euler_successes = 0
for i in range(num_test):
# generate random
q = tf.random_quaternion()
# test euler_from_quaternion
e1 = tf.euler_from_quaternion(q)
e2 = euler_from_quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
quaternion_successes += int(np.allclose(e1, e2))
# test quaternion_from_euler
q1 = tf.quaternion_from_euler(e1[0], e1[1], e1[2])
q2 = quaternion_from_euler(roll=e2[0], pitch=e2[1], yaw=e2[2])
euler_successes += int(np.allclose(q1, q2))
print(' -> Quaternion successes: {}/{}'.format(
quaternion_successes, num_test))
print(' -> Euler successes: {}/{}'.format(euler_successes, num_test))
print("--------------------\n")
tot_successess += quaternion_successes + euler_successes
tot_test += num_test * 2
