def _instantiate_node(surfaces, model, node, transform, texcache):
rot = node['rot']
transform = tf.concatenate_matrices(transform,
tf.translation_matrix(node['offset']),
_rotation_matrix(rot))
if node['mesh'] != -1:
mesh_transform = tf.concatenate_matrices(
transform, tf.translation_matrix(node['pivot']))
mesh = model.meshes[node['mesh']]
for k, surface in mesh.items():
try:
material_name = model.materials[surface['material']]
except:
continue # if there's no material, don't render the surface
surfaces.append({
'vertices':
_transform_vertices(mesh_transform, surface['vertices']),
'material':
texcache.load(material_name)
})
for child in node['children']:
_instantiate_node(surfaces, model, child, transform, texcache)
def init_points(self):
if self._tf_buffer.can_transform('odom', 'front_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'front_point', Time())
self.front_point.position.x = transform.transform.translation.x
self.front_point.position.y = transform.transform.translation.y
self.front_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'front_point', tf.translation_matrix((0, 0, 0)))
if self._tf_buffer.can_transform('odom', 'rear_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'rear_point', Time())
self.rear_point.position.x = transform.transform.translation.x
self.rear_point.position.y = transform.transform.translation.y
self.rear_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'rear_point', tf.translation_matrix((0, 0, 0)))
while self.pub.get_subscription_count() < 1:
pass
sleep(2)
self.publish_transform('odom', 'base_link', self.base_link_start_height)
def callback_camera_pose(msg):
model_name = msg.name
model_pose = msg.pose
for i in range(0, 3):
model_name[i]
if model_name[i] == 'realsense_camera':
pose_cam_x = model_pose[i].position.x
pose_cam_y = model_pose[i].position.y
pose_cam_z = model_pose[i].position.z
orient_cam_x = model_pose[i].orientation.x
orient_cam_y = model_pose[i].orientation.y
orient_cam_z = model_pose[i].orientation.z
orient_cam_w = model_pose[i].orientation.w
elif model_name[i] == 'ce_jacket':
pose_jac_x = model_pose[i].position.x
pose_jac_y = model_pose[i].position.y
pose_jac_z = model_pose[i].position.z
orient_jac_x = model_pose[i].orientation.x
orient_jac_y = model_pose[i].orientation.y
orient_jac_z = model_pose[i].orientation.z
orient_jac_w = model_pose[i].orientation.w
Transl_cam = tf.translation_matrix(
np.array((pose_cam_x, pose_cam_y, pose_cam_z)))
Transl_jac = tf.translation_matrix(
np.array((pose_jac_x, pose_jac_y, pose_jac_z)))
Rot_cam = np.array(
[orient_cam_x, orient_cam_y, orient_cam_z, orient_cam_w])
def __init__(self):
super().__init__('executor')
"""Update frequency
Single targets: {baudrate} / ({uart frame length} * {cmd length} * {number of joint})
= 115200 / (9 * 4 * 18) ~= 177Hz <- autodetect-baudrate on maestro
= 200000 / (9 * 4 * 1x8) ~= 300Hz <- fixed-baudrate on maestro
Multiple targets: {baudrate} / ({uart frame length} * ({header length} + {mini-cmd length} * {number of joints}))
= 115200 / (9 * (3 + 2 * 18)) ~= 320Hz
= 200000 / (9 * (3 + 2 * 18)) ~= 560Hz"""
self.subscription = self.create_subscription(
Path,
'sparse_path',
self.new_trajectory,
10
)
self.spi_bridge = self.create_publisher(
UInt8MultiArray, 'stm32_cmd', 10
)
self.steps_trajectory = []
self.init_test_path()
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
self.broadcaster = self.create_publisher(TFMessage, '/tf', 10)
self.publish_transform = get_transform_publisher(self.broadcaster, self.get_clock())
self.pub = self.create_publisher(JointState, 'joint_states', 10)
self.base_link_start_height = tf.translation_matrix((0.0, 0.0, 0.1))
self.path_proxy = PathProxy(self.steps_trajectory, self.get_logger())
self.trajectory_generator = TrajectoryGenerator(self.path_proxy)
self.trajectory_encoder = TrajectoryEncoder()
self.front_point = Pose()
self.rear_point = Pose()
self.init_points()
self.inv_kin_calc = TripodInverseKinematics(self._tf_buffer, self.get_clock())
self.prev_time = self.get_clock().now()
first_step = TrajectoryPoint()
first_step.timestamp = self.prev_time.nanoseconds / 10**9
first_step.left_moving = True
first_step.left_pose = tf.translation_matrix((0, 0, 0))
first_step.right_pose = tf.translation_matrix((0, 0, 0))
self.prev_trajectory = self.trajectory_generator.generate_trajectory(
first_step)
self.first_time = False
self.timer_callback()
self.first_time = False
self.start_timer = self.create_timer(START_DELAY, self.timer_callback)
self.timer = None
self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback)
def to_homogeneous(vec):
d = [float(x) for x in vec]
if len(d) != 7 and len(d) != 3:
raise ValueError("Only poses with 3 or 7 elements supported! this one has %d" % len(d))
if len(d) == 7:
return concatenate_matrices( translation_matrix(d[0:3]), quaternion_matrix([d[6]]+d[3:6]) )
return concatenate_matrices( translation_matrix(d[0:2]+[0]), rotation_matrix(d[2], [0,0,1]) )
def generate_leg(leg: str, params: list):
front_signs = {'front': '', 'rear': '-'}
side_signs = {'left': '', 'right': '-'}
lower_limits = {'left': '0', 'right': str(-pi)}
upper_limits = {'left': str(pi), 'right': '0'}
with open('../urdf/leg.urdf.template', 'r') as file:
leg_template = Template(file.read())
mappings = {'l_width': 0.02, 'leg': leg}
front, side = leg.split('_')
mappings['front_sign'] = front_signs[front]
mappings['side_sign'] = side_signs[side]
mappings['lower_limit'] = lower_limits[side]
mappings['upper_limit'] = upper_limits[side]
for param in params:
try:
d = float(param['d'])
except ValueError:
d = 0.0
try:
th = float(param['th'])
except ValueError:
th = 0.0
try:
a = float(param['a'])
except ValueError:
a = 0.0
try:
al = float(param['al'])
except ValueError:
al = 0.0
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(th, zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
matrix = concatenate_matrices(tz, rz, tx, rx)
rpy = euler_from_matrix(matrix)
xyz = translation_from_matrix(matrix)
name = param['joint']
mappings[f'{name}_j_xyz'] = '{} {} {}'.format(*xyz)
mappings[f'{name}_j_rpy'] = '{} {} {}'.format(*rpy)
mappings[f'{name}_l_xyz'] = '{} 0 0'.format(xyz[0] / 2.)
mappings[f'{name}_l_rpy'] = '0 0 0'
mappings[f'{name}_l_len'] = str(a)
return leg_template.substitute(mappings)
def __init__(self, transformation_plane):
origo = np.array([0, 0, 0])
xp = transformation_plane.x_axis()
yp = transformation_plane.y_axis()
zp = Point3.Cross(xp, yp)
zp = Point3.Normalize(zp)
# y' projected y on yz (normal=x)
X1 = np.array([1, 0, 0]) # XAxisWorld
X2 = np.array([0, 1, 0]) # YAxisWorld
X3 = np.array([0, 0, 1]) # ZAxisWorld
# These vectors are the local X,Y,Z of the rotated object
X1Prime = xp.ToArr()
X2Prime = yp.ToArr()
X3Prime = zp.ToArr()
# This matrix will transform points
# from the world to the rotated axis
aWorldToLocalTransform = np.matrix([
[np.dot(X1Prime, X1),
np.dot(X1Prime, X2),
np.dot(X1Prime, X3), 0],
[np.dot(X2Prime, X1),
np.dot(X2Prime, X2),
np.dot(X2Prime, X3), 0],
[np.dot(X3Prime, X1),
np.dot(X3Prime, X2),
np.dot(X3Prime, X3), 0], [0, 0, 0, 1]
])
matrix2 = translation_matrix(-1 * transformation_plane.origin())
self.WorldToLocalTransform = aWorldToLocalTransform * matrix2
# This matrix will transform points
# from the rotated axis back to the world
aLocalToWorldTransform = np.matrix([
[np.dot(X1, X1Prime),
np.dot(X1, X2Prime),
np.dot(X1, X3Prime), 0],
[np.dot(X2, X1Prime),
np.dot(X2, X2Prime),
np.dot(X2, X3Prime), 0],
[np.dot(X3, X1Prime),
np.dot(X3, X2Prime),
np.dot(X3, X3Prime), 0], [0, 0, 0, 1]
])
matrix4 = translation_matrix(transformation_plane.origin())
self.LocalToWorldTransform = matrix4 * aLocalToWorldTransform
def triangle(self, center: ndarray,
left_side: bool) -> Tuple[ndarray, ndarray, ndarray]:
mult = 1 if left_side else -1
x_shift = 0.15
side_y_shift = mult * 0.17
mid_y_shift = mult * -0.2
z = 0.0
front_point = tf.concatenate_matrices(
center, tf.translation_matrix((x_shift, side_y_shift, z)))
mid_point = tf.concatenate_matrices(
center, tf.translation_matrix((0.0, mid_y_shift, z)))
rear_point = tf.concatenate_matrices(
center, tf.translation_matrix((-x_shift, side_y_shift, z)))
return front_point, mid_point, rear_point
def __init__( self , fovy , ratio , near , far , robot_files ) : self.fovy = fovy self.near = near self.far = far self.ratio = ratio self.camera = None self.plane = Plane( (2,2) ) self.wall = Plane( (20,10) ) self.mw = tr.rotation_matrix( -m.pi / 2.0 , (1,0,0) ) self.mw = np.dot( self.mw , tr.translation_matrix( (0,3,0) ) ) self.robot = Robot( robot_files ) self.x = 0.0 self.last_time = timer() self.plane_alpha = 65.0 / 180.0 * m.pi self.lpos = [ 1 ,-1 , 0 ] self._make_plane_matrix() self.draw_robot = True self.draw_sparks = True self.draw_front = False self.draw_back = False
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
rospy.logdebug("joint: %s"%jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0,0,0]
if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def get_bbox2d_from_bbox3d(cam_proj_matrix, rx, ry, rz, sl, sh, sw, tx, ty,
tz):
bbox_to_velo_matrix = np.matmul(
transformations.translation_matrix([tx, ty, tz]),
transformations.euler_matrix(rx, ry, rz, 'sxyz'))
bbox_proj_matrix = np.matmul(cam_proj_matrix, bbox_to_velo_matrix)
vertices = np.empty([2, 2, 2, 2])
for k in [0, 1]:
for l in [0, 1]:
for m in [0, 1]:
v = np.array([(k - 0.5) * sl, -(l) * sh, (m - 0.5) * sw, 1.])
v = np.matmul(bbox_proj_matrix, v)
vertices[k, l, m, :] = spherical_project(v[0], v[1], v[2])
vertices = vertices.astype(np.int32)
x1 = np.amin(vertices[:, :, :, 0])
x2 = np.amax(vertices[:, :, :, 0])
y1 = np.amin(vertices[:, :, :, 1])
y2 = np.amax(vertices[:, :, :, 1])
return (x1, y1, x2, y2)
def rule_branch(self): ''' @brief Create a branch ''' # Translate frame = self.current_node.frame frame = np.dot(frame, tr.translation_matrix([0, 0, self.current_node.size])) # Add noise if self.noise != 0: self.current_phi_angle += random.uniform(-self.noise, self.noise) self.current_rho_angle += random.uniform(-self.noise, self.noise) # Phi Rotation phi_matrix = tr.rotation_matrix(self.current_phi_angle, [0, 0, 1]) frame[:3,:3] = np.dot(frame[:3,:3], phi_matrix[:3,:3]) # Rho rotation rho_matrix = tr.rotation_matrix(self.current_rho_angle, [1, 0, 0]) frame[:3,:3] = np.dot(frame[:3,:3], rho_matrix[:3,:3]) # Create a new node node = Node(frame, self.current_node.size * self.scale_factor, self.current_node.diameter * self.current_subdivision_ratio * self.scale_factor) self.current_node.add_node(node) self.current_node = node self.current_rho_angle = 0 self.current_phi_angle = 0 self.current_subdivision_ratio = 1
def recebe(msg):
global x
global y
global z
global id
for marker in msg.markers:
x = round(marker.pose.pose.position.x, 2)
y = round(marker.pose.pose.position.y, 2)
z = round(marker.pose.pose.position.z, 2)
#print(x)
id = marker.id
#print(marker.pose.pose)
if id == 100:
print(
buffer.can_transform("base_link", "ar_marker_100",
rospy.Time(0)))
header = Header(frame_id="ar_marker_100")
trans = buffer.lookup_transform("base_link", "ar_marker_100",
rospy.Time(0))
t = transformations.translation_matrix([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
r = transformations.quaternion_matrix([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
m = numpy.dot(r, t)
v2 = numpy.dot(m, [0, 0, 1, 0])
v2_n = v2[0:-1]
n2 = v2_n / linalg.norm(v2_n)
cosa = numpy.dot(n2, [1, 0, 0])
print("ang", math.degrees(math.acos(cosa)))
def calibrate_extrinsics_depth(frame,K,rect,pi0,pi1,shift,z_direction=-1):
"""calibrating depth camera w.r.t. ground plane
frame - depth frame
K - camera matrix
rect - [u,v,w,h] image rectangle that contains plane points
pi0 - (u,v) image point corresponding to the 0,0,0
pi1 - (u,v) image point corresponding to the direction 0,1,0
z_directon - -1 or 1, for the direction of the z
returns Pwc, Pcw - transformation matrices world->camera, camera->world
"""
u0,v0,h,w = rect
uv = np.array(zip(*(x.flat for x in np.mgrid[u0:u0+h,v0:v0+w])))
z = frame[uv[:,0], uv[:,1]].reshape((-1,1))
indices = np.where(z != 0)[0]
uvz = np.hstack([uv[indices], z[indices]])
pts_c = image_to_camera(uvz[:,1], uvz[:,0], uvz[:,2], K)
plane, ctr = estimate_plane(pts_c, 1)
plane *= z_direction
# the direction for X (rows)
u,v = pi0
p0 = image_to_camera(v, u, frame[u,v], K)
u,v = pi1
p1 = image_to_camera(v, u, frame[u,v], K)
Pcw = estimate_camera_to_plane(plane, pts_c, p0, p1)
# tune this to move coordinate system
T = translation_matrix(np.array(shift))
Pwc = la.pinv(Pcw).dot(T)
Pcw = la.pinv(Pwc)
return Pwc, Pcw
def sample_point_cloud(self, num):
"""Generate a sampling of the points from all the vertices using a
reverse area integral
"""
sample = np.empty((num,3), dtype='f')
norm = np.empty((num,3), dtype='f')
if not 'area' in self.__dict__:
self.calculate_area_lookup()
rnds = np.random.rand(num) * self.area.total
inds = [bisect.bisect_left(self.area.cumsum, rnd) for rnd in rnds]
coords = np.random.rand(num,2).astype('f')
for i, ind, (r1,r2) in zip(range(num), inds, coords):
v1,v2,v3 = self.vertices[self.faces[ind,:3],:3]
if r1+r2 >= 1: r1,r2 = 1-r1,1-r2
point = v1 + (v2-v1)*r1 + (v3-v1)*r2
sample[i,:] = point
normal = np.cross(v2-v1,v3-v1)
normal /= np.sqrt(np.dot(normal,normal))
norm[i,:] = normal
# Center all the points to make it easier to apply handbuilt rotations
import transformations
T1 = transformations.translation_matrix(sample.mean(0))
RT = np.dot(self.RT, T1)
sample -= sample.mean(0)
return pointmodel.PointModel(sample, norm, RT)
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
rospy.logdebug("joint: %s" % jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0, 0, 0]
if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]
parentFromChild = conversions.trans_rot_to_hmat(
joint.origin.position,
transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(
valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(
np.array(joint.axis) * valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,
joint.child,
ns=ns,
valuedict=valuedict)
def check_points_in_range(current_rob_pos, other_rob_pos):
# calculate bottom right corner of the bounding rectangle
rx, ry, rth = current_rob_pos
rth = (rth / 100) + (pi / 2)
zx = round(rx - (m * sin(rth)), 3)
zy = round(ry + (m * cos(rth)), 3)
# calculating the transformation matrix of the new frame at that vertex
alpha, beta, gamma = 0, 0, (-(pi / 2 - rth))
origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
Rx = transformations.rotation_matrix(alpha, xaxis)
Ry = transformations.rotation_matrix(beta, yaxis)
Rz = transformations.rotation_matrix(gamma, zaxis)
T = transformations.translation_matrix([zx, zy, 0])
R = transformations.concatenate_matrices(Rx, Ry, Rz)
Z = transformations.shear_matrix(beta, xaxis, origin, zaxis)
S = transformations.scale_matrix(1, origin)
M = transformations.concatenate_matrices(T, R, Z, S)
M = np.linalg.inv(M)
# calculating the new point in that frame
other_x = other_rob_pos[0]
other_y = other_rob_pos[1]
other_z = 0
other_h = 1
other_point_homogenous = np.array([other_x, other_y, other_z, other_h])
new_point = np.dot(M, other_point_homogenous)
new_point = np.round(new_point, 3)
# checking that the point lies within the boundaries
px, py, pz, ph = new_point
if px <= (2 * m) and px >= 0 and py <= m and py >= 0:
return 1
else:
return 0
def __init__(self, filename='ipm/camera_info.yaml'):
self.info = self.load_camera_info(osp.join(PKG_PATH, filename))
self.frame_width = self.info['width']
self.frame_height = self.info['height']
# Intrinsics
self.K = np.zeros((3, 4))
self.K[:, :3] = np.asarray(self.info['intrinsics']['K']).reshape(3, 3)
self.D = np.asarray(self.info['intrinsics']['D'])
# Extrinsics
self.extrinsics_euler = np.radians(self.info['extrinsics']['rpy'])
self.extrinsics_rotation = euler_matrix(self.extrinsics_euler[0],
self.extrinsics_euler[1],
self.extrinsics_euler[2])
self.extrinsics_translation = translation_matrix(
self.info['extrinsics']['position'])
# Overwrite calibration data if available
if 'calibration' not in self.info:
print(
'ERROR: Calibration not performed, run InversePerspectiveMapping.calibrate() first!'
)
return
else:
print('Loading calibration data from file...')
self.ipm_matrix = np.asarray(
self.info['calibration']['ipm_matrix']).reshape(3, 3)
self.ipm_matrix_inv = np.linalg.inv(self.ipm_matrix)
self.ipm_image_dims = tuple(
self.info['calibration']['ipm_image_dims'][::-1])
self.ipm_px_to_m = self.info['calibration']['ipm_px_to_m']
self.ipm_m_to_px = self.info['calibration']['ipm_m_to_px']
self.calibration_ego_y = self.info['calibration'][
'calibration_ego_y']
def recebe(msg):
global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado
global y
global z
global id
for marker in msg.markers:
x = round(marker.pose.pose.position.x, 2) # para arredondar casas decimais e impressao ficar ok
y = round(marker.pose.pose.position.y, 2)
z = round(marker.pose.pose.position.z, 2)
#print(x)
id = marker.id
#print(marker.pose.pose)
if id == 100:
print(buffer.can_transform("base_link", "ar_marker_100", rospy.Time(0)))
header = Header(frame_id= "ar_marker_100")
# Procura a transformacao em sistema de coordenadas entre a base do robo e o marcador numero 100
# Note que para seu projeto 1 voce nao vai precisar de nada que tem abaixo, a
# Nao ser que queira levar angulos em conta
trans = buffer.lookup_transform("base_link", "ar_marker_100", rospy.Time(0))
# Separa as translacoes das rotacoes
t = transformations.translation_matrix([trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z])
# Encontra as rotacoes
r = transformations.quaternion_matrix([trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w])
m = numpy.dot(r,t)
v2 = numpy.dot(m,[0,0,1,0])
v2_n = v2[0:-1]
n2 = v2_n/linalg.norm(v2_n)
cosa = numpy.dot(n2,[1,0,0])
angulo_marcador_robo = math.degrees(math.acos(cosa))
print("Angulo entre marcador e robo", angulo_marcador_robo)
def getMarkerTFFromMap(m):
# This doesn't work
mid = "Marker%s" % m
marker = marker_dict[mid]
w_mat = None
if marker is not None:
w = 0 # east
p = (-marker["x"], -marker["y"], -0.25) #east
if marker["wall"] == "north":
p = (marker["y"], -marker["x"], -0.25)
w = math.pi / 2
elif marker["wall"] == "south":
p = (-marker["y"], marker["x"], -0.25)
w = -math.pi / 2
elif marker["wall"] == "west":
p = (marker["x"], marker["y"], -0.25)
w = math.pi
q = tr.quaternion_from_euler(0, 0, w)
print("Map: (%s) Marker%s: %s %s" % (marker["wall"], m, p, q))
t = tr.translation_matrix(p)
r = tr.quaternion_matrix(q)
w_mat = numpy.dot(t, r)
return (w_mat, None)
def _unpack(x):
trans = x[:3]
eulxyz = x[3:]
T = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.euler_matrix(eulxyz[0], eulxyz[1],
eulxyz[2], 'sxyz'))
return T
def send_msg(self):
if self.msg_sent:
return
print('Sending msg')
self.msg_sent = True
front_point = self.transform('odom', 'front_point')
inc = tf.concatenate_matrices(
tf.rotation_matrix(-5 * pi / 180, (0, 0, 1)),
tf.translation_matrix((0.03, 0.01, 0)),
)
path = [tf.concatenate_matrices(front_point, inc)]
for i in range(179):
path.append(tf.concatenate_matrices(path[-1], inc))
msg = Path()
msg.header.frame_id = 'odom'
msg.header.stamp.sec = int(self.get_clock().now().nanoseconds / 10**9)
msg.header.stamp.nanosec = self.get_clock().now().nanoseconds % 10**9
poses = [matrix_to_pose(point) for point in path]
for pose in poses:
pose_stamped = PoseStamped()
pose_stamped.header = msg.header
pose_stamped.pose = pose
msg.poses.append(pose_stamped)
self.pub.publish(msg)
print('Sent')
def __init__(self, fovy, ratio, near, far, robot_files):
self.fovy = fovy
self.near = near
self.far = far
self.ratio = ratio
self.camera = None
self.plane = Plane((2, 2))
self.wall = Plane((20, 10))
self.mw = tr.rotation_matrix(-m.pi / 2.0, (1, 0, 0))
self.mw = np.dot(self.mw, tr.translation_matrix((0, 3, 0)))
self.robot = Robot(robot_files)
self.x = 0.0
self.last_time = timer()
self.plane_alpha = 65.0 / 180.0 * m.pi
self.lpos = [1, -1, 0]
self._make_plane_matrix()
self.draw_robot = True
self.draw_sparks = True
self.draw_front = False
self.draw_back = False
def pose_to_matrix(pose: Pose) -> ndarray:
rot = pose.orientation
trans = pose.position
rot_matrix = tf.quaternion_matrix((rot.w, rot.x, rot.y, rot.z))
trans_matrix = tf.translation_matrix((trans.x, trans.y, trans.z))
return tf.concatenate_matrices(trans_matrix, rot_matrix)
def coil_transform_pos(pos):
pos[1] = -pos[1]
a, b, g = np.radians(pos[3:])
r_ref = tf.euler_matrix(a, b, g, 'sxyz')
t_ref = tf.translation_matrix(pos[:3])
m_img = tf.concatenate_matrices(t_ref, r_ref)
return m_img
def _x2transform(self, x):
trans, eulxyz, scale = self._unpack(x)
origin = [0, 0, 0]
T = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.euler_matrix(eulxyz[0], eulxyz[1],
eulxyz[2], 'sxyz'),
tf.scale_matrix(scale, origin))
return T
def compute_transform(rx, rz, tx, ty, tz): origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tf.rotation_matrix(rx, xaxis) Rz = tf.rotation_matrix(rz, zaxis) T = tf.translation_matrix([tx, ty, tz]) return T.dot(Rz).dot(Rx)
def _make_plane_matrix(self):
r = tr.rotation_matrix(self.plane_alpha, (0, 0, 1))
s = tr.scale_matrix(1)
t = tr.translation_matrix((-1.25, .7, .05))
self.m = np.dot(np.dot(t, s), r)
self.im = la.inv(self.m)
self.im[3] = [0, 0, 0, 1]
def _make_plane_matrix( self ) : r = tr.rotation_matrix( self.plane_alpha , (0,0,1) ) s = tr.scale_matrix( 1 ) t = tr.translation_matrix( (-1.25,.7,.05) ) self.m = np.dot( np.dot( t , s ) , r ) self.im = la.inv( self.m ) self.im[3] = [ 0 , 0 , 0 , 1 ]
def make_next_step(self, prev_step: TrajectoryPoint) -> TrajectoryPoint:
point = TrajectoryPoint()
point.timestamp = prev_step.timestamp + 1 / SERVO_FREQ
phase = prev_step.phase + 1 / (SERVO_FREQ * GAIT_PERIOD)
z_lift = self.leg_lift * sin(phase * pi)
current_lift = tf.translation_matrix((0.0, 0.0, z_lift))
if phase > 1.0:
phase = 0
point.left_pose = prev_step.next_left_stand
point.right_pose = prev_step.next_right_stand
# TODO after sending to stm, publish first element as frontPoint
next_stand = self.path_proxy.get_point(point.timestamp)
if prev_step.left_moving:
point.prev_left_stand = prev_step.next_left_stand
point.next_left_stand = prev_step.next_left_stand
point.prev_right_stand = prev_step.next_right_stand
point.next_right_stand = next_stand
else:
point.prev_right_stand = prev_step.next_right_stand
point.next_right_stand = prev_step.next_right_stand
point.prev_left_stand = prev_step.next_left_stand
point.next_left_stand = next_stand
point.left_moving = not prev_step.left_moving
else:
if prev_step.left_moving:
left_shift = interpolate(prev_step.prev_left_stand,
prev_step.next_left_stand,
phase)
point.left_pose = tf.concatenate_matrices(
# prev_step.prev_left_stand,
left_shift,
current_lift)
point.right_pose = prev_step.right_pose
else:
right_shift = interpolate(prev_step.prev_right_stand,
prev_step.next_right_stand,
phase)
point.right_pose = tf.concatenate_matrices(
# prev_step.prev_right_stand,
right_shift,
current_lift)
point.left_pose = prev_step.left_pose
point.next_left_stand = prev_step.next_left_stand
point.next_right_stand = prev_step.next_right_stand
point.prev_left_stand = prev_step.prev_left_stand
point.prev_right_stand = prev_step.prev_right_stand
point.left_moving = prev_step.left_moving
point.phase = phase
point.base_link_pose = midpoint(point.left_pose, point.right_pose)
point.base_link_pose[2, 3] = self.base_height
return point
def global_pose(matrix, x_ob, y_ob, angle):
#obtiene el angulo del tag con respecto al mapa
q1 = math.atan2(y_ob, x_ob)
# invierte el angulo del tag segun el plano del mapa
angle = -angle
# Calcula la distancia del robot al tag
z = dist(matrix)
# Calcula la distancia del tag al mapa
d = math.sqrt(x_ob**2 + y_ob**2)
# Calcula el angulo del robot c/r a q1
q2 = angle2(q1, angle, tf.euler_from_matrix(matrix))
R1 = tf.rotation_matrix(q1, [0, 0, 1])
T1 = tf.translation_matrix([d, 0, 0])
R2 = tf.rotation_matrix(q2, [0, 0, 1])
T2 = tf.translation_matrix([z, 0, 0])
result = R1.dot(T1.dot(R2.dot(T2.dot([0, 0, 0, 1]))))
return result
def transform_pad(padcoords, pad_length, pad_separation, pad_thickness,
drum_separation, drum_radius, totalAlpha):
TranG = trn.translation_matrix((padcoords[0], padcoords[1], padcoords[2]))
if padcoords[4] != 0:
RotG = trn.rotation_matrix(padcoords[4], [0, 1, 0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(((pad_length + pad_separation), 0, 0))
if (padcoords[0] + pad_separation + pad_length) > (drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0] - pad_separation - pad_length) < -(drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -2 * math.pi:
alpha -= (totalAlpha[0] - 2 * math.pi)
totalAlpha[0] = -2 * math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else:
Final = trn.concatenate_matrices(TranG, RotG, TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3], Final[1][3], Final[2][3], 0, angle, 0]
return padcoords
def RotateVectorArr(
self, q_arr,
p_arr): #geometry_msgs/Quaternion and geometry_msgs/Point
angle_transform = transformations.quaternion_matrix(q_ar)
p_transform = transformations.translation_matrix(p_arr)
new_p_transform = np.dot(angle_transform, p_transform)
new_p_arr = new_p_transform[:3, 3]
return new_p_arr
def filter_points_inside_box(points, dim, loc, ry):
box_T = transformations.translation_matrix(loc)
box_R = transformations.rotation_matrix(ry, [0,1,0])
box_S = np.diag([dim[2], dim[0], dim[1], 1.0]) # transformations.scale_matrix(dim)
inv_mat = np.linalg.inv(np.dot(box_T, np.dot(box_R, box_S))) # post multiply
points_box_trans = np.dot(inv_mat, points.T).T
inside_idxs = (points_box_trans[:, 0] >= -0.5) & (points_box_trans[:, 0] <= 0.5) & \
(points_box_trans[:, 1] >= -0.5) & (points_box_trans[:, 1] <= 0.5) & \
(points_box_trans[:, 2] >= -0.5) & (points_box_trans[:, 2] <= 0.5)
return points_box_trans[inside_idxs], inside_idxs
def make_transform_matrix(quaternion, translation):
M = np.identity(4)
T = transformations.translation_matrix(translation)
M = np.dot(M, T)
R = transformations.quaternion_matrix(quaternion)
M = np.dot(M, R)
M /= M[3, 3]
return M
def _draw(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearColor(0.0, 0.0, 0.0, 1.0)
view_mat = translation_matrix((0, 0, -self.dist))
view_mat = view_mat.dot(self.ball.matrix())
view_mat = view_mat.dot(scale_matrix(self.zoom))
self.VMatrix = view_mat
self.draw_hook()
OpenGL.GLUT.glutSwapBuffers()
def calc_error(args, debug=False):
# angle_x, angle_y, o_x, o_y, o_z = args
angle_x, angle_y = args
x = np.array([1, 0, 0, 1])
R_x = transformations.rotation_matrix(angle_x, [1, 0, 0], eye_centre)
R_y = transformations.rotation_matrix(angle_y, [0, 1, 0], eye_centre)
S = transformations.scale_matrix(6)
T = transformations.translation_matrix(eye_centre)
# T = transformations.translation_matrix(eye_centre + np.array([o_x*100, o_y*100, o_z*100]))
T2 = transformations.translation_matrix([0,0,-12])
if debug:
trans = transformations.concatenate_matrices(*reversed([T, T2, R_x, R_y]))
pt = dehomo(trans.dot([0,0,-50,1]))
cv2.line(img,
tuple(dehomo(cam_mat.dot(coord_swap.dot(true_iris_centre_3d))).astype(int)),
tuple(dehomo(cam_mat.dot(coord_swap.dot(pt))).astype(int)),
(255, 255, 0))
est_pts = []
for t in np.linspace(0, np.pi*2, accuracy):
R = transformations.rotation_matrix(t, [0, 0, 1])
trans = transformations.concatenate_matrices(*reversed([R, S, T, T2, R_x, R_y]))
threeD_pt = coord_swap.dot(dehomo(trans.dot(x)))
pt = cam_mat.dot(threeD_pt)
if point_in_poly(dehomo(pt).astype(int), data["ldmks_lids_2d"]):
est_pts.append(dehomo(pt).astype(int))
if debug: cv2.circle(img, tuple(dehomo(pt).astype(int)), 1, (255, 0, 0), -1)
try:
D = cdist(est_pts, visible_pts, 'euclidean')
H1 = np.max(np.min(D, axis=1))
H2 = np.max(np.min(D, axis=0))
return (H1 + H2) / 2.0
except ValueError:
return 20
def setPersp(scale_factor = 1, distort_location = '/afs/cs.stanford.edu/u/twangcat/scratch/sail-car-log/process/'):
distort = { }
distort['M_1'] = [np.eye(3)] # perspective distrotion in cam1 image space
distort['M_2'] = [np.eye(3)] # perspective distrotion in cam2 image space
distort['T'] = [np.eye(4)] # corresponding real-world transformation
distort['T'].insert(0,translation_matrix(np.array([1.0,0,0])))
distort['T'].insert(0,translation_matrix(np.array([-1.0,0,0])))
distort['T'].insert(0,euler_matrix(0, 0.06, 0))
distort['T'].insert(0,euler_matrix(0, -0.06, 0))
distort['T'].insert(0,euler_matrix(0, 0.12, 0))
distort['T'].insert(0,euler_matrix(0, -0.12, 0))
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam1.pickle'))) # shift 1.0
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam1.pickle'))) # shift -1.0
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam1.pickle','r'))) # rotate 0.06
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam1.pickle','r'))) # rotate -0.06
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam1.pickle','r'))) # rotate 0.12
distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam1.pickle','r'))) # rotate -0.12
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam2.pickle'))) # shift 1.0
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam2.pickle'))) # shift -1.0
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam2.pickle','r'))) # rotate 0.06
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam2.pickle','r'))) # rotate -0.06
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam2.pickle','r'))) # rotate 0.12
distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam2.pickle','r'))) # rotate -0.12
assert len(distort['M_1']) == len(distort['M_2']) and len(distort['M_1'])==len(distort['T'])
if scale_factor != 1:
for i in xrange(len(distort['M_1'])-1): # last one is always identity, no need change.
distort['M_1'][i][:,0:2]/=scale_factor
distort['M_1'][i][2,:]/=scale_factor
distort['M_2'][i][:,0:2]/=scale_factor
distort['M_2'][i][2,:]/=scale_factor
return distort
def transform(self, pitch, yaw):
"""Gives the translation matrix associated with this arm with the given pitch
and yaw.
"""
tLength = tr.translation_matrix(np.array([self.length, 0, 0]))
rPitch = tr.rotation_matrix(pitch, yaxis)
rYaw = tr.rotation_matrix(yaw, zaxis)
#return armLength * rPitch * rYaw
return tr.concatenate_matrices(rYaw, rPitch, tLength)
def getWorldMarkerMatrixFromTF(m):
global listener
marker_link = "Marker%s__link" % m
try:
tw = listener.getLatestCommonTime(marker_link, 'world')
(t, r) = listener.lookupTransform(marker_link, 'world', tw)
print("TF: Marker%s: %s %s" % (m, t, r))
w_mat = numpy.dot(tr.translation_matrix(t), tr.quaternion_matrix(r))
except:
return (None, None)
return (w_mat, tw)
def transform_pad(padcoords,pad_length,pad_separation,pad_thickness,drum_separation,drum_radius,totalAlpha):
TranG = trn.translation_matrix(( padcoords[0],padcoords[1],padcoords[2] ))
if padcoords[4]!=0:
RotG = trn.rotation_matrix(padcoords[4],[0,1,0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(( (pad_length+pad_separation),0,0))
if (padcoords[0]+pad_separation+pad_length) >(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0]-pad_separation-pad_length)<-(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-2*math.pi:
alpha -= (totalAlpha[0] - 2*math.pi)
totalAlpha[0] = -2*math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else :
Final = trn.concatenate_matrices(TranG,RotG,TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3],Final[1][3],Final[2][3],0,angle,0]
return padcoords
def __init__(self):
# sagital
self.RAnkleRotY = None
self.LAnkleRotY = None
self.RKneeRotY = None
self.LKneeRotY = None
self.RHipRotY = None
self.LHipRotY = None
self.AbsRotY = None
self.BustRotY = None
self.FootT = tf.translation_matrix((0, 0, L_FOOT_Z))
self.ThighT = tf.translation_matrix((0, 0, L_THIGH))
self.LegT = tf.translation_matrix((0, 0, L_LEG))
self.HipT = tf.translation_matrix((0, 0, L_HIP_Z))
self.AbsT = tf.translation_matrix((0, 0, L_ABS_Z))
self.BustT = tf.translation_matrix((0, 0, L_BUST_Z))
# Totally guessed com of segments
self.SegCom = array([[0.025, 0, L_FOOT_Z, 1],
[0, 0, L_THIGH, 1],
[0, 0, L_LEG, 1],
[0, 0, 3.0 * L_HIP_Z / 4.0, 1],
[- 0.02, 0, 3.0 * L_ABS_Z / 4.0, 1],
[- 0.02, 0, 3.0 * L_BUST_Z / 4.0, 1]])
# Totally guessed masses of segments (roughtly the mass of the motors)
self.Masses = array([2 * (2 * MX28_MASS), 2 * (MX28_MASS), 2 *
(MX28_MASS), 2 * (2 * MX28_MASS), (2 * MX64_MASS), 13 * MX28_MASS])
self.TotalMass = self.Masses.sum()
self.Masses /= self.TotalMass
self.roty = None
self.xaxis = (1, 0, 0)
self.yaxis = (0, 1, 0)
self.zaxis = (0, 0, 1)
def controls_3d(self, dx, dy, zooming_one_shot=False):
""" Orbiting the camera is implemented the following way:
- the rotation is split into a rotation around the *world* Z axis
(controlled by the horizontal mouse motion along X) and a
rotation around the *X* axis of the camera (pitch) *shifted to
the focal origin* (the world origin for now). This is controlled
by the vertical motion of the mouse (Y axis).
- as a result, the resulting transformation of the camera in the
world frame C' is:
C' = (T · Rx · T⁻¹ · (Rz · C)⁻¹)⁻¹
where:
- C is the original camera transformation in the world frame,
- Rz is the rotation along the Z axis (in the world frame)
- T is the translation camera -> world (ie, the inverse of the
translation part of C
- Rx is the rotation around X in the (translated) camera frame """
CAMERA_TRANSLATION_FACTOR = 0.01
CAMERA_ROTATION_FACTOR = 0.01
if not (self.is_rotating or self.is_panning or self.is_zooming):
return
current_pos = self.current_cam.transformation[:3, 3].copy()
distance = numpy.linalg.norm(self.focal_point - current_pos)
if self.is_rotating:
rotation_camera_x = dy * CAMERA_ROTATION_FACTOR
rotation_world_z = dx * CAMERA_ROTATION_FACTOR
world_z_rotation = transformations.euler_matrix(0, 0, rotation_world_z)
cam_x_rotation = transformations.euler_matrix(rotation_camera_x, 0, 0)
after_world_z_rotation = numpy.dot(world_z_rotation, self.current_cam.transformation)
inverse_transformation = transformations.inverse_matrix(after_world_z_rotation)
translation = transformations.translation_matrix(
transformations.decompose_matrix(inverse_transformation)[3])
inverse_translation = transformations.inverse_matrix(translation)
new_inverse = numpy.dot(inverse_translation, inverse_transformation)
new_inverse = numpy.dot(cam_x_rotation, new_inverse)
new_inverse = numpy.dot(translation, new_inverse)
self.current_cam.transformation = transformations.inverse_matrix(new_inverse).astype(numpy.float32)
if self.is_panning:
tx = -dx * CAMERA_TRANSLATION_FACTOR * distance
ty = dy * CAMERA_TRANSLATION_FACTOR * distance
cam_transform = transformations.translation_matrix((tx, ty, 0)).astype(numpy.float32)
self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform)
if self.is_zooming:
tz = dy * CAMERA_TRANSLATION_FACTOR * distance
cam_transform = transformations.translation_matrix((0, 0, tz)).astype(numpy.float32)
self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform)
if zooming_one_shot:
self.is_zooming = False
self.update_view_camera()
NOM = NOM.T #NOM_A = NOM_A.T MEAS = MEAS.T #MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" #print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) #R = transformations.inverse_matrix(R) print "Rotation matrix calulated from points difference\n\n",R,"\n" rot=transformations.inverse_matrix(R) rob = transformations.euler_matrix(math.radians(OLDBASE[3]),math.radians(OLDBASE[4]),math.radians(OLDBASE[5]), axes='sxyz') tob = transformations.translation_matrix(OLDBASE[0:3]) robn = np.dot(tob,rob) #rotation matrix from old base print "Rotation matrix from old base\n\n",robn,"\n" robnew = np.dot(rot,robn) print "Rotation matrix RESULT\n\n",robnew,"\n" scale, shear, angles, translate, perspective = transformations.decompose_matrix(robnew) roll = math.degrees(angles[0]) pitch = math.degrees(angles[1]) yaw = math.degrees(angles[2]) print "OLD BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(OLDBASE[0],OLDBASE[1],OLDBASE[2],OLDBASE[3],OLDBASE[4],OLDBASE[5]) print "NEW BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(translate[0],translate[1],translate[2],roll,pitch,yaw) print "----- COPY & PASTE -----\n\n[Base]" print "BaseX = %.3f" % translate[0] print "BaseY = %.3f" % translate[1]
def _xyz_to_mat44(self, pos):
return transformations.translation_matrix((pos['x'], pos['y'], pos['z']))
def transform_from_quat(pos, quat):
T = tf.concatenate_matrices(tf.translation_matrix(pos),
tf.quaternion_matrix(quat))
return T
def transform(pos, eulxyz):
ex, ey, ez = eulxyz * np.pi / 180
return tf.concatenate_matrices(tf.translation_matrix(pos),
tf.euler_matrix(ex, ey, ez, 'sxyz'))
def transform(R, t):
Tr = tf.identity_matrix()
Tr[:3, :3] = R
return tf.concatenate_matrices(Tr, tf.translation_matrix(t))
def getMatrix(self): return rotation_matrix(90 + self.rot[1], [1.0, 0.0, 0.0]).dot(rotation_matrix(-self.rot[0], [0.0, 0.0, 1.0]).dot(translation_matrix(-self.pos)))
MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) scale, shear, angles, translate, perspective = transformations.decompose_matrix(R) #R = transformations.inverse_matrix(R) print "Rotation matrix\n\n",R,"\n" #rot=R.T p1 = transformations.euler_matrix(MEAS_A[0,0]/180*np.pi,MEAS_A[1,0]/180*np.pi,MEAS_A[2,0]/180*np.pi, axes='sxyz') p2 = transformations.euler_matrix(MEAS_A[0,1]/180*np.pi,MEAS_A[1,1]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p3 = transformations.euler_matrix(MEAS_A[0,2]/180*np.pi,MEAS_A[1,2]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p4 = transformations.euler_matrix(MEAS_A[0,3]/180*np.pi,MEAS_A[1,3]/180*np.pi,MEAS_A[2,3]/180*np.pi, axes='sxyz') t1 = transformations.translation_matrix(MEAS[0:,0]) t2 = transformations.translation_matrix(MEAS[0:,1]) t3 = transformations.translation_matrix(MEAS[0:,2]) t4 = transformations.translation_matrix(MEAS[0:,3]) #print "p1\n",p1,"\n","p2\n",p2,"\n","p3\n",p3,"\n","p4\n",p4,"\n" #print "t1\n",t1,"\n","t2\n",t2,"\n","t3\n",t3,"\n","t4\n",t4,"\n" p1n = np.dot(t1,p1) p2n = np.dot(t2,p2) p3n = np.dot(t3,p3) p4n = np.dot(t4,p4) print "p1n\n",p1n,"\n","p2n\n",p2n,"\n","p3n\n",p3n,"\n","p4n\n",p4n,"\n" p1n = np.dot(R,p1n) p2n = np.dot(R,p2n) p3n = np.dot(R,p3n) p4n = np.dot(R,p4n) print "p1n\n",p1n,"\n","p2n\n",p2n,"\n","p3n\n",p3n,"\n","p4n\n",p4n,"\n"
def mkpln( s , p , r , a , c ) : p = Plane((s,s),np.dot(tr.translation_matrix(p),tr.rotation_matrix(r*m.pi/180.0,a))) p.c = c return p
