def test_png_export():
"""Test if the graph can be exported to PNG."""
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3,
0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(
active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
_, filename = tempfile.mkstemp(".png")
try:
tm.write_png(filename)
assert_true(os.path.exists(filename))
except ImportError:
raise SkipTest("pydot is required for this test")
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.tm = TransformManager()
self.tm.add_transform(
"origin", "world",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0.0]),
[0.0, 0.0, 0.0]))
def test_request_added_transform():
"""Request an added transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
A2B_2 = tm.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B_2)
def test_transform_not_added():
"""Test request for transforms that have not been added."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
C2D = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("C", "D", C2D)
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "A", "G")
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "G", "D")
assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform,
"A", "D")
def test_update_transform():
"""Update an existing transform."""
random_state = np.random.RandomState(0)
A2B1 = random_transform(random_state)
A2B2 = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B1)
tm.add_transform("A", "B", A2B2)
A2B = tm.get_transform("A", "B")
# Hack: test depends on internal member
assert_array_almost_equal(A2B, A2B2)
assert_equal(len(tm.i), 1)
assert_equal(len(tm.j), 1)
def test_pickle():
"""Test if a transform manager can be pickled."""
random_state = np.random.RandomState(1)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
_, filename = tempfile.mkstemp(".pickle")
try:
pickle.dump(tm, open(filename, "wb"))
tm2 = pickle.load(open(filename, "rb"))
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
A2B2 = tm2.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B2)
def test_png_export_without_pydot_fails():
"""Test graph export without pydot."""
pydot_available = transform_manager.pydot_available
tm = TransformManager()
try:
transform_manager.pydot_available = False
assert_raises_regexp(ImportError,
"pydot must be installed to use this feature.",
tm.write_png, "bla")
finally:
transform_manager.pydot_available = pydot_available
def test_has_frame():
"""Check if frames have been registered with transform."""
tm = TransformManager()
tm.add_transform("A", "B", np.eye(4))
assert_true(tm.has_frame("A"))
assert_true(tm.has_frame("B"))
assert_false(tm.has_frame("C"))
def test_deactivate_transform_manager_precision_error():
A2B = np.eye(4)
A2B[0, 0] = 2.0
A2B[3, 0] = 3.0
tm = TransformManager()
assert_raises_regexp(ValueError, "Expected rotation matrix",
tm.add_transform, "A", "B", A2B)
if int(platform.python_version()[0]) == 2:
# Python 2 seems to incorrectly suppress some warnings, not sure why
n_expected_warnings = 7
else:
n_expected_warnings = 9
try:
warnings.filterwarnings("always", category=UserWarning)
with warnings.catch_warnings(record=True) as w:
tm = TransformManager(strict_check=False)
tm.add_transform("A", "B", A2B)
tm.add_transform("B", "C", np.eye(4))
tm.get_transform("C", "A")
assert_equal(len(w), n_expected_warnings)
finally:
warnings.filterwarnings("default", category=UserWarning)
def setParams(self, params):
self.num_cameras = params["num_cameras"]
self.print = params["print"] == "true"
self.cameras_dict = {}
for i in range(int(self.num_cameras)) :
device_serial = params["device_serial_"+str(i)]
self.name = params["name_"+str(i)]
rx = np.radians(float(params["rx_"+str(i)]))
ry = np.radians(float(params["ry_"+str(i)]))
rz = np.radians(float(params["rz_"+str(i)]))
tx = float (params["tx_"+str(i)])
ty = float(params["ty_"+str(i)])
tz = float(params["tz_"+str(i)])
# Transforms: world -> slam_sensor -> robot -> origin
# (measure) (sensor pose) (robot base wrt world origin)
tm = TransformManager()
tm.add_transform("robot", "origin",
pytr.transform_from(pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0.0]),
[0.0, 0.0, 0.0]))
tm.add_transform("slam_sensor", "robot",
pytr.transform_from(pyrot.active_matrix_from_intrinsic_euler_xyz([rx, ry, rz]),
[tx, ty, tz]))
self.cameras_dict[self.name] = [device_serial, tm]
# realsense configuration
try:
for key in self.cameras_dict:
config = rs.config()
config.enable_device(self.cameras_dict[key][0])
config.enable_stream(rs.stream.pose)
pipeline = rs.pipeline()
pipeline.start(config)
self.cameras_dict[key].append(pipeline)
except Exception as e:
print("Error initializing camera")
print(e)
sys.exit(-1)
data_list=[]
angle_list = []
mapper_value = 0
tracker_value = 0
for key in self.cameras_dict:
self.cameras_dict[key].append(data_list)
self.cameras_dict[key].append(angle_list)
self.cameras_dict[key].append(mapper_value)
self.cameras_dict[key].append(tracker_value)
return True
def test_whitelist():
"""Test correct handling of whitelists for plotting."""
random_state = np.random.RandomState(2)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
nodes = tm._whitelisted_nodes(None)
assert_equal(set(["A", "B"]), nodes)
nodes = tm._whitelisted_nodes("A")
assert_equal(set(["A"]), nodes)
assert_raises_regexp(KeyError, "unknown nodes", tm._whitelisted_nodes, "C")
def test_remove_transform():
tm = TransformManager()
tm.add_transform("A", "B", np.eye(4))
tm.add_transform("C", "D", np.eye(4))
assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform,
"A", "D")
tm.add_transform("B", "C", np.eye(4))
tm.get_transform("A", "C")
tm.remove_transform("B", "C")
tm.remove_transform("B", "C") # nothing should happen
assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform,
"A", "D")
tm.get_transform("B", "A")
tm.get_transform("D", "C")
def test_deactivate_checks():
tm = TransformManager(check=False)
tm.add_transform("A", "B", np.zeros((4, 4)))
tm.add_transform("B", "C", np.zeros((4, 4)))
A2B = tm.get_transform("A", "C")
assert_array_almost_equal(A2B, np.zeros((4, 4)))
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.plot_utils import make_3d_axis
from pytransform3d.transformations import random_transform
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
A2world = random_transform(random_state)
B2world = random_transform(random_state)
A2C = random_transform(random_state)
D2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "world", A2world)
tm.add_transform("B", "world", B2world)
tm.add_transform("A", "C", A2C)
tm.add_transform("D", "B", D2B)
plt.figure(figsize=(10, 5))
ax = make_3d_axis(3, 121)
ax = tm.plot_frames_in("world", ax=ax, alpha=0.6)
ax.view_init(30, 20)
ax = make_3d_axis(3, 122)
ax = tm.plot_frames_in("A", ax=ax, alpha=0.6)
ax.view_init(30, 20)
def test_connected_components():
"""Test computation of connected components in the graph."""
tm = TransformManager()
tm.add_transform("A", "B", np.eye(4))
assert_equal(tm.connected_components(), 1)
tm.add_transform("D", "E", np.eye(4))
assert_equal(tm.connected_components(), 2)
tm.add_transform("B", "C", np.eye(4))
assert_equal(tm.connected_components(), 2)
tm.add_transform("D", "C", np.eye(4))
assert_equal(tm.connected_components(), 1)
return a0, a1, a2
try:
config = rs.config()
config.enable_device("925122110807")
config.enable_stream(rs.stream.pose)
pipeline = rs.pipeline()
pipeline.start(config)
except Exception as e:
print("Error initializing camera")
print(e)
sys.exit(-1)
tm = TransformManager()
tm.add_transform(
"robot", "origin",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0.0]),
[2000, -30000, 0.0]))
tm.add_transform(
"slam_sensor", "robot",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0]),
[0.0, 0.0, 0.0]))
while True:
frames = pipeline.wait_for_frames()
f = frames.first_or_default(rs.stream.pose)
def test_request_concatenated_transform():
"""Request a concatenated transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
B2C = random_transform(random_state)
F2A = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("B", "C", B2C)
tm.add_transform("D", "E", np.eye(4))
tm.add_transform("F", "A", F2A)
A2C = tm.get_transform("A", "C")
assert_array_almost_equal(A2C, concat(A2B, B2C))
C2A = tm.get_transform("C", "A")
assert_array_almost_equal(
C2A, concat(invert_transform(B2C), invert_transform(A2B)))
F2B = tm.get_transform("F", "B")
assert_array_almost_equal(F2B, concat(F2A, A2B))
automatically.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (random_quaternion, matrix_from_euler_xyz,
q_id)
from pytransform3d.transformations import transform_from_pq, transform_from
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(matrix_from_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
ee2object = tm.get_transform("end-effector", "object")
ax = tm.plot_frames_in("robot", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
plt.show()
def check_trans_matrix_from_file(file_path):
import matplotlib.pyplot as plt
import pytransform3d.transformations as ptrans
from pytransform3d.transform_manager import TransformManager
from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
data = np.load(file_path)
cam_T_marker_mats_R = data['arr_0']
cam_T_marker_mats_t = data['arr_1']
EE_T_base_mats_R = data['arr_2']
EE_T_base_mats_t = data['arr_3']
cam_T_base_R, cam_T_base_t = cv2.calibrateHandEye(EE_T_base_mats_R,
EE_T_base_mats_t,
cam_T_marker_mats_R,
cam_T_marker_mats_t)
# chane (3,1) shape to (3, )
cam_T_base_t = cam_T_base_t.squeeze(1)
print("cam_T_base_R:\n", cam_T_base_R.shape, "\n", cam_T_base_R)
print("cam_T_base_t:\n", cam_T_base_t.shape, "\n", cam_T_base_t)
base_T_cam_R = cam_T_base_R.transpose()
base_T_cam_t = -cam_T_base_R.transpose().dot(cam_T_base_t)
x_errs = []
y_errs = []
z_errs = []
abs_errs = []
trans_pts = []
base_T_EE_mats_t = []
for i, EE_T_base_t in enumerate(EE_T_base_mats_t):
base_T_EE_t = -EE_T_base_mats_R[i].transpose().dot(EE_T_base_t)
base_T_EE_mats_t.append(base_T_EE_t) # For visualization
print("Point in base: ", i, " ",
base_T_EE_t.transpose().shape, " ", base_T_EE_t)
print("Point in camera: ", i, " ", cam_T_marker_mats_t[i].shape, " ",
cam_T_marker_mats_t[i])
trans_pt = np.add(cam_T_base_R.dot(cam_T_marker_mats_t[i]),
cam_T_base_t)
trans_pts.append(trans_pt)
print("Transed point: ", trans_pt.shape, trans_pt)
x_errs.append(abs(trans_pt[0] - base_T_EE_t[0]))
y_errs.append(abs(trans_pt[1] - base_T_EE_t[1]))
z_errs.append(abs(trans_pt[2] - base_T_EE_t[2]))
abs_errs.append(
np.sqrt(
np.square(trans_pt[0] - base_T_EE_t[0]) +
np.square(trans_pt[1] - base_T_EE_t[1]) +
np.square(trans_pt[2] - base_T_EE_t[2])))
print("X-axis error: ", "max: ", max(x_errs), "min: ", min(x_errs),
"mean: ",
sum(x_errs) / len(x_errs))
print("Y-axis error: ", "max: ", max(y_errs), "min: ", min(y_errs),
"mean: ",
sum(y_errs) / len(y_errs))
print("Z-axis error: ", "max: ", max(z_errs), "min: ", min(z_errs),
"mean: ",
sum(z_errs) / len(z_errs))
print("Abs error: ", "max: ", max(abs_errs), "min: ", min(abs_errs),
"mean: ",
sum(abs_errs) / len(abs_errs))
abs_errs = np.sort(abs_errs)
plt.figure(1)
plt.plot(range(len(abs_errs)), abs_errs)
fig_3d = plt.figure(2)
ax = fig_3d.add_subplot(111, projection='3d')
for pt in cam_T_marker_mats_t:
ax.scatter(pt[0], pt[1], pt[2], marker='^')
for pt in trans_pts:
ax.scatter(pt[0], pt[1], pt[2], marker='.')
plt.figure(3)
base_T_cam = ptrans.transform_from(base_T_cam_R, base_T_cam_t, True)
tm = TransformManager()
tm.add_transform("base", "cam", base_T_cam)
axis = tm.plot_frames_in("base", s=0.3)
plt.show()
"""
=====================
Transformation Editor
=====================
The transformation editor can be used to manipulate transformations.
"""
from pytransform3d.transform_manager import TransformManager
from pytransform3d.editor import TransformEditor
from pytransform3d.transformations import transform_from
from pytransform3d.rotations import active_matrix_from_extrinsic_euler_xyx
tm = TransformManager()
tm.add_transform(
"tree", "world",
transform_from(active_matrix_from_extrinsic_euler_xyx([0, 0.5, 0]),
[0, 0, 0.5]))
tm.add_transform(
"car", "world",
transform_from(active_matrix_from_extrinsic_euler_xyx([0.5, 0, 0]),
[0.5, 0, 0]))
te = TransformEditor(tm, "world", s=0.3)
te.show()
print("tree to world:")
print(te.transform_manager.get_transform("tree", "world"))
#!/usr/bin/python3
import numpy as np
from pytransform3d.transform_manager import TransformManager
import pytransform3d.transformations as pytr
import pytransform3d.rotations as pyrot
tm = TransformManager()
# place the world with rotation and translationa as seen from origin
tm.add_transform("origin", "world", pytr.transform_from(pyrot.active_matrix_from_extrinsic_euler_xyz([0, 0, 0]), [0.0, 0.0, 0]))
# place the robot with rotation and translation as seen from world
tm.add_transform("world", "robot", pytr.transform_from(pyrot.active_matrix_from_intrinsic_euler_xyz([0, 0, np.pi/2.0]), [-10.0, 0, 0]))
t = tm.get_transform("world", "robot")
print(t)
# transform the nose of the robot to world CS
p = pytr.transform(t, np.array([0, 10, 0, 1.0]))
print(p)
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.tm = TransformManager()
self.tm.add_transform(
"origin", "world",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0.0]),
[0.0, 0.0, 0.0]))
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../../etc/informatica.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.robot_object = Shape("Pioneer")
self.back_left_wheel = Joint("p3at_back_left_wheel_joint")
self.back_right_wheel = Joint("p3at_back_right_wheel_joint")
self.front_left_wheel = Joint("p3at_front_left_wheel_joint")
self.front_right_wheel = Joint("p3at_front_right_wheel_joint")
self.radius = 110 # mm
self.semi_width = 140 # mm
# cameras
self.cameras_write = {}
self.cameras_read = {}
self.front_left_camera_name = "pioneer_camera_left"
cam = VisionSensor(self.front_left_camera_name)
self.cameras_write[self.front_left_camera_name] = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.front_right_camera_name = "pioneer_camera_right"
cam = VisionSensor(self.front_right_camera_name)
self.cameras_write[self.front_right_camera_name] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.cameras_read = self.cameras_write.copy()
self.mutex_c = Lock()
# PoseEstimation
self.robot_full_pose_write = RoboCompFullPoseEstimation.FullPoseEuler()
self.robot_full_pose_read = RoboCompFullPoseEstimation.FullPoseEuler()
self.mutex = Lock()
self.ldata = []
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
def compute(self):
tc = TimeControl(0.05)
while True:
self.pr.step()
self.read_cameras(
[self.front_left_camera_name, self.front_right_camera_name])
self.read_joystick()
self.read_robot_pose()
self.move_robot()
tc.wait()
###########################################
### CAMERAS get and publish cameras data
###########################################
def read_cameras(self, camera_names):
for camera_name in camera_names:
cam = self.cameras_write[camera_name]
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(True)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
#image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"],
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
cameraID=cam["id"],
width=cam["handle"].get_resolution()[0],
height=cam["handle"].get_resolution()[1],
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
depthFactor=1.0,
depth=depth.tobytes())
self.mutex_c.acquire()
self.cameras_write, self.cameras_read = self.cameras_read, self.cameras_write
self.mutex_c.release()
# try:
# self.camerargbdsimplepub_proxy.pushRGBD(cam["rgb"], cam["depth"])
# except Ice.Exception as e:
# print(e)
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata: # and (time.time() - self.joystick_newdata[1]) > 0.1:
datos = self.joystick_newdata[0]
adv = 0.0
rot = 0.0
for x in datos.axes:
if x.name == "advance":
adv = x.value if np.abs(x.value) > 10 else 0
if x.name == "rotate" or x.name == "turn":
rot = x.value if np.abs(x.value) > 0.01 else 0
converted = self.convert_base_speed_to_motors_speed(adv, rot)
print("Joystick ", [adv, rot], converted)
self.joystick_newdata = None
self.last_received_data_time = time.time()
else:
elapsed = time.time() - self.last_received_data_time
if elapsed > 2 and elapsed < 3:
self.convert_base_speed_to_motors_speed(0, 0)
def convert_base_speed_to_motors_speed(self, adv, rot):
# adv = r*(Wl + Wr)/2
# rot = r*(-Wl + Wr)/2c
# isolating Wl,Wr
# Wl = ( adv - c*rot ) / r
# Wr = ( adv + c*rot ) / r
left_vel = (adv + self.semi_width * rot) / self.radius
right_vel = (adv - self.semi_width * rot) / self.radius
self.back_left_wheel.set_joint_target_velocity(left_vel)
self.back_right_wheel.set_joint_target_velocity(right_vel)
self.front_left_wheel.set_joint_target_velocity(left_vel)
self.front_right_wheel.set_joint_target_velocity(right_vel)
return left_vel, right_vel
###########################################
### ROBOT POSE get and publish robot position
###########################################
def read_robot_pose(self):
slam_0 = Shape("slam_0")
pose = slam_0.get_position()
rot = slam_0.get_orientation()
linear_vel, ang_vel = slam_0.get_velocity()
isMoving = np.abs(linear_vel[0]) > 0.01 or np.abs(
linear_vel[1]) > 0.01 or np.abs(ang_vel[2]) > 0.01
self.bState = RoboCompGenericBase.TBaseState(
x=pose[0] * 1000,
z=pose[1] * 1000,
alpha=rot[2] - np.pi,
advVx=linear_vel[0] * 1000,
advVz=linear_vel[1] * 1000,
rotV=ang_vel[2],
isMoving=isMoving)
# self.tm.add_transform("world", "robot", pytr.transform_from(pyrot.active_matrix_from_intrinsic_euler_xyz
# ([rot[0], rot[1], rot[2]-np.pi]),
# [pose[0]*1000.0, pose[1]*1000.0, pose[2]*1000.0]
# ))
#
# t = self.tm.get_transform("origin", "robot")
# angles = pyrot.extrinsic_euler_xyz_from_active_matrix(t[0:3, 0:3])
self.robot_full_pose_write.x = pose[0] * 1000
self.robot_full_pose_write.y = pose[1] * 1000
self.robot_full_pose_write.z = pose[2] * 1000
self.robot_full_pose_write.rx = rot[0]
self.robot_full_pose_write.ry = rot[1]
self.robot_full_pose_write.rz = rot[2] - np.pi
self.robot_full_pose_write.vx = linear_vel[0] * 1000.0
self.robot_full_pose_write.vy = linear_vel[1] * 1000.0
self.robot_full_pose_write.vz = linear_vel[2] * 1000.0
self.robot_full_pose_write.vrx = ang_vel[0]
self.robot_full_pose_write.vry = ang_vel[1]
self.robot_full_pose_write.vrz = ang_vel[2]
# swap
self.mutex.acquire()
self.robot_full_pose_write, self.robot_full_pose_read = self.robot_full_pose_read, self.robot_full_pose_write
self.mutex.release()
###########################################
### MOVE ROBOT from Omnirobot interface
###########################################
def move_robot(self):
if self.speed_robot != self.speed_robot_ant: # or (isMoving and self.speed_robot == [0,0,0]):
self.convert_base_speed_to_motors_speed(self.speed_robot[0],
self.speed_robot[1])
# print("Velocities sent to robot:", self.speed_robot)
self.speed_robot_ant = self.speed_robot
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
if camera in self.cameras_read.keys():
return RoboCompCameraRGBDSimple.TRGBD(
self.cameras_read[camera]["rgb"],
self.cameras_read[camera]["depth"])
else:
e = RoboCompCameraRGBDSimple.HardwareFailedException()
e.what = "No camera found with this name: " + camera
raise e
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
if camera in self.cameras_read.keys():
return self.cameras_read[camera]["depth"]
else:
e = RoboCompCameraRGBDSimple.HardwareFailedException()
e.what = "No camera found with this name: " + camera
raise e
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
if camera in self.cameras_read.keys():
return self.cameras_read[camera]["rgb"]
else:
e = RoboCompCameraRGBDSimple.HardwareFailedException()
e.what = "No camera found with this name: " + camera
raise e
##############################################
## Omnibase
#############################################
#
# correctOdometer
#
def DifferentialRobot_correctOdometer(self, x, z, alpha):
pass
#
# getBasePose
#
def DifferentialRobot_getBasePose(self):
#
# implementCODE
#
x = self.bState.x
z = self.bState.z
alpha = self.bState.alpha
return [x, z, alpha]
#
# getBaseState
#
def DifferentialRobot_getBaseState(self):
return self.bState
#
# resetOdometer
#
def DifferentialRobot_resetOdometer(self):
pass
#
# setOdometer
#
def DifferentialRobot_setOdometer(self, state):
pass
#
# setOdometerPose
#
def DifferentialRobot_setOdometerPose(self, x, z, alpha):
pass
#
# setSpeedBase
#
def DifferentialRobot_setSpeedBase(self, advz, rot):
self.speed_robot = self.convert_base_speed_to_motors_speed(advz, rot)
#
# stopBase
#
def DifferentialRobot_stopBase(self):
pass
# ===================================================================
# CoppeliaUtils
# ===================================================================
def CoppeliaUtils_addOrModifyDummy(self, type, name, pose):
if not Dummy.exists(name):
dummy = Dummy.create(0.1)
# one color for each type of dummy
if type == RoboCompCoppeliaUtils.TargetTypes.Info:
pass
if type == RoboCompCoppeliaUtils.TargetTypes.Hand:
pass
if type == RoboCompCoppeliaUtils.TargetTypes.HeadCamera:
pass
dummy.set_name(name)
else:
dummy = Dummy(name)
parent_frame_object = None
if type == RoboCompCoppeliaUtils.TargetTypes.HeadCamera:
parent_frame_object = Dummy("viriato_head_camera_pan_tilt")
# print("Coppelia ", name, pose.x/1000, pose.y/1000, pose.z/1000)
dummy.set_position(
[pose.x / 1000., pose.y / 1000., pose.z / 1000.],
parent_frame_object)
dummy.set_orientation([pose.rx, pose.ry, pose.rz],
parent_frame_object)
# =============== Methods for Component Implements ==================
# ===================================================================
#
# IMPLEMENTATION of getFullPose method from FullPoseEstimation interface
#
def FullPoseEstimation_getFullPoseEuler(self):
return self.robot_full_pose_read
def FullPoseEstimation_getFullPoseMatrix(self):
t = self.tm.get_transform("origin", "robot")
m = RoboCompFullPoseEstimation.FullPoseMatrix()
m.m00 = t[0][0]
m.m01 = t[0][1]
m.m02 = t[0][2]
m.m03 = t[0][3]
m.m10 = t[1][0]
m.m11 = t[1][1]
m.m12 = t[1][2]
m.m13 = t[1][3]
m.m20 = t[2][0]
m.m21 = t[2][1]
m.m22 = t[2][2]
m.m23 = t[2][3]
m.m30 = t[3][0]
m.m31 = t[3][1]
m.m32 = t[3][2]
m.m33 = t[3][3]
return m
#
# IMPLEMENTATION of setInitialPose method from FullPoseEstimation interface
#
def FullPoseEstimation_setInitialPose(self, x, y, z, rx, ry, rz):
# should move robot in Coppelia to designated pose
self.tm.add_transform(
"origin", "world",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([rx, ry, rz]),
[x, y, z]))
#
# IMPLEMENTATION of getAllSensorDistances method from Ultrasound interface
#
def Ultrasound_getAllSensorDistances(self):
ret = RoboCompUltrasound.SensorsState()
#
# write your CODE here
#
return ret
#
# IMPLEMENTATION of getAllSensorParams method from Ultrasound interface
#
def Ultrasound_getAllSensorParams(self):
ret = RoboCompUltrasound.SensorParamsList()
#
# write your CODE here
#
return ret
#
# IMPLEMENTATION of getBusParams method from Ultrasound interface
#
def Ultrasound_getBusParams(self):
ret = RoboCompUltrasound.BusParams()
#
# write your CODE here
#
return ret
#
# IMPLEMENTATION of getSensorDistance method from Ultrasound interface
#
def Ultrasound_getSensorDistance(self, sensor):
ret = int()
#
# write your CODE here
#
return ret
#
# IMPLEMENTATION of getSensorParams method from Ultrasound interface
#
def Ultrasound_getSensorParams(self, sensor):
ret = RoboCompUltrasound.SensorParams()
#
# write your CODE here
#
return ret
# ===================================================================
# ===================================================================
#
# IMPLEMENTATION of getRSSIState method from RSSIStatus interface
#
def RSSIStatus_getRSSIState(self):
ret = RoboCompRSSIStatus.TRSSI()
ret.percentage = 100
return ret
#
# IMPLEMENTATION of getBatteryState method from BatteryStatus interface
#
def BatteryStatus_getBatteryState(self):
ret = RoboCompBatteryStatus.TBattery()
ret.percentage = 100
return ret
def test_check_consistency():
"""Test correct detection of inconsistent graphs."""
random_state = np.random.RandomState(2)
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
B2A = random_transform(random_state)
tm.add_transform("B", "A", B2A)
assert_false(tm.check_consistency())
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
assert_true(tm.check_consistency())
C2D = random_transform(random_state)
tm.add_transform("C", "D", C2D)
assert_true(tm.check_consistency())
B2C = random_transform(random_state)
tm.add_transform("B", "C", B2C)
assert_true(tm.check_consistency())
A2D_over_path = tm.get_transform("A", "D")
A2D = random_transform(random_state)
tm.add_transform("A", "D", A2D)
assert_false(tm.check_consistency())
tm.add_transform("A", "D", A2D_over_path)
assert_true(tm.check_consistency())
"""
=====================
Transformation Editor
=====================
The transformation editor can be used to manipulate transformations.
"""
from pytransform3d.transform_manager import TransformManager
from pytransform3d.editor import TransformEditor
from pytransform3d.transformations import transform_from
from pytransform3d.rotations import matrix_from_euler_xyz
tm = TransformManager()
tm.add_transform(
"tree", "world",
transform_from(matrix_from_euler_xyz([0, 0.5, 0]), [0, 0, 0.5]))
tm.add_transform(
"car", "world",
transform_from(matrix_from_euler_xyz([0.5, 0, 0]), [0.5, 0, 0]))
te = TransformEditor(tm, "world", s=0.3)
te.show()
print("tree to world:")
print(te.transform_manager.get_transform("tree", "world"))