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 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 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