def transform_scanlines(self):
print('Transforming pointcloud')
l2t = self.get_laser_to_turntable_transform()
w = self.angspeed
st = self.start_scan_time
sl = self.scanlines
slt = self.scanline_times
self.pc = np.empty((len(sl)*len(sl[0]), 3))
pci = 0
for i in range(len(sl)):
udp = self.undistort_points(sl[i])
# x,z in laser coordinates
self.ptp = self.perspective_transform(udp)
angle = w * (slt[i] - st)
rot_trf = m3d.Transform(m3d.Orientation.new_rot_z(-angle), m3d.Vector()) * l2t
for p in self.ptp:
# create m3d Vector of each point in scanline
p = m3d.Vector(p[0], 0, p[1])
# transform to turntable coordinates
tp = rot_trf * p
self.pc[pci] = tp.data
pci += 1
def paintGL(self):
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
glu.gluPerspective(45.0, 4.0 / 3.0, 0.1, 100.0)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
glu.gluLookAt(4, 3, 10, 0, 0, 0, 0, 1, 0)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self._vbo)
gl.glVertexPointer(3, gl.GL_FLOAT, 0, None)
gl.glLinkProgram(self._shader_program)
gl.glUseProgram(self._shader_program)
model = math3d.Matrix44.translate(0, 0, 0)
view = math3d.Matrix44.look_at_right_hand(math3d.Vector(4, 3, 10),
math3d.Vector(0, 0, 0),
math3d.Vector(0, 1, 0))
projection = math3d.Matrix44.projection_right_hand(
math.radians(45), 4.0 / 3.0, 0.1, 100.0)
mvp = model * view * projection
mvp_id = gl.glGetUniformLocation(self._shader_program, "mvp")
gl.glUniformMatrix4fv(mvp_id, 1, gl.GL_FALSE, mvp.raw_data())
gl.glDrawArrays(gl.GL_TRIANGLES, 0, self._vertices.size)
def new_vec_to_vec(cls, from_vec, to_vec):
"""Factory for a new orientation which is the minimal rotation which
rotates 'from_vec' to 'to_vec'. Technically the axis-angle
that can be computed for the two vectors.
"""
# Ensure that the vectors are m3d unit vectors
from_vec = m3d.Vector(from_vec).normalized
to_vec = m3d.Vector(to_vec).normalized
# Angle between the vectords
angle = from_vec.angle(to_vec)
# Consider cases
if angle <= utils.sqrt_eps:
# // Identity
return Orientation()
elif angle < np.pi - utils.sqrt_eps:
# // Regular, minimal rotation
return Orientation(angle * from_vec.cross(to_vec).normalized)
else:
# // Find any suitable rotation axis
x_angle = Vector.ex.angle(from_vec)
if x_angle > 1e-3 and x_angle < np.pi - 1.0e-3:
return Orientation(angle *
Vector.ex.cross(from_vec).normalized)
y_angle = Vector.ey.angle(from_vec)
if y_angle > 1e-3 and y_angle < np.pi - 1.0e-3:
return Orientation(angle *
Vector.ey.cross(from_vec).normalized)
z_angle = Vector.ez.angle(from_vec)
if z_angle > 1e-3 and z_angle < np.pi - 1.0e-3:
return Orientation(angle *
Vector.ez.cross(from_vec).normalized)
def new_csys_from_xpy(self):
"""
Restores and returns new coordinate system calculated from three points: X, Origin, Y
based on math3d: Transform.new_from_xyp
"""
# Set coord. sys. to 0
self.csys = m3d.Transform()
print(
"A new coordinate system will be defined from the next three points"
)
print("Firs point is X, second Origin, third Y")
print(
"Set it as a new reference by calling myrobot.set_csys(new_csys)")
input("Move to first point and click Enter")
# we do not use get_pose so we avoid rounding values
pose = URRobot.getl(self)
print("Introduced point defining X: {}".format(pose[:3]))
px = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Origo: {}".format(pose[:3]))
p0 = m3d.Vector(pose[:3])
input("Move to third point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Y: {}".format(pose[:3]))
py = m3d.Vector(pose[:3])
new_csys = m3d.Transform.new_from_xyp(px - p0, py - p0, p0)
self.set_csys(new_csys)
return new_csys
def __new_csys_from_planes(self):
"""
first plane define Z axis
second plane define X axis
third plane define Y axis
Z directed outside plane
X and Y axis directed inside plane
"""
planes = []
for i in range(3):
p1, p2, p3 = self.__get_three_points_from_robot()
planes.append(get_plane_from_three_points(p1, p2, p3))
self.__check_cross_direction(planes[i])
orig = intersection_of_three_planes(planes[0], planes[1], planes[2])
orig[2] -= self.MODEL_HEIGHT
vec_x = m3d.Vector(planes[1][:3])
vec_y = m3d.Vector(planes[2][:3])
csys = m3d.Transform.new_from_xyp(vec_x, vec_y, orig)
self.ROBOT.set_csys(csys)
def __init__(self, **kwargs):
"""Supported, named constructor arguments:
* 'point_direction': An ordered pair of vectors representing a
point on the line and the line's direction.
* 'point', 'direction': Separate vectors for point on line and
direction of line in named arguments.
* 'point0', 'point1': Two points defining the line, in
separate named arguments.
"""
if 'point_direction' in kwargs:
self._p, self._d = [
m3d.Vector(e) for e in kwargs['point_direction']
]
elif 'point' in kwargs and 'direction' in kwargs:
self._p = m3d.Vector(kwargs['point'])
self._d = m3d.Vector(kwargs['direction'])
elif 'point0' in kwargs and 'point1' in kwargs:
self._p = m3d.Vector(kwargs['point0'])
self._d = m3d.Vector(kwargs['point1']) - self._p
else:
raise Exception(
'Can not create Line object on given arguments: "{}"'.format(
kwargs))
# Create the unit direction vector
self._ud = self._d.normalized
def get_tcp_ft(self, wait=True):
"""
get force and torque of tcp in tcp frame
"""
b2force = self.get_tcp_force(wait)
tcp2b = m3d.Transform(self.rtmon.getTCF(wait).tolist()).orient.inverse
ft = (tcp2b * m3d.Vector(b2force[:3])).list + (tcp2b * m3d.Vector(b2force[3:])).list
return ft
def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
set tool to given pos, keeping constant orientation
"""
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
def speedl(self, velocities, acc, min_time):
"""
move at given velocities in base csys until minimum min_time seconds
"""
#r = m3d.Transform(velocities)
v = self.csys.orient * m3d.Vector(velocities[:3])
w = self.csys.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True):
"""
set tool to given pos, keeping constant orientation
"""
if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.apply_transform(trans, acc, vel, radius, wait=wait)
def speedl(self, velocities, acc, min_time):
"""
move at given velocities until minimum min_time seconds
"""
v = self.csys.orient * m3d.Vector(velocities[:3])
w = self.csys.orient * m3d.Vector(velocities[3:])
vels = np.concatenate((v.array, w.array))
return self.speedx("speedl", vels, acc, min_time)
def speedl_tool(self, velocities, acc, min_time):
"""
move at given velocities in tool csys until minimum min_time seconds
"""
pose = self.get_pose()
v = pose.orient * m3d.Vector(velocities[:3])
w = pose.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
def translate(self, vect, acc=None, vel=None, radius=0, wait=True):
"""
move tool in base coordinate, keeping orientation
"""
t = m3d.Transform()
if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect)
t.pos += m3d.Vector(vect)
return self.add_transform_base(t, acc, vel, radius, wait=wait)
def fit_plane(self, points):
"""Compute the plane vector from a set of points. 'points'
must be an array of row position vectors, such that
points[i] is a position vector."""
centre = np.sum(points, axis=0) / len(points)
eigen = np.linalg.eig(np.cov(points.T))
min_ev_i = np.where(eigen[0] == min(eigen[0]))[0][0]
normal = eigen[1].T[min_ev_i]
(self._p, self._n) = (m3d.Vector(centre), m3d.Vector(normal))
def speedx(self, command, velocities, acc, min_time):
"""
send command to robot formated like speedl or speedj
move at given velocities until minimum min_time seconds
"""
v = self.csys.orient * m3d.Vector(velocities[:3])
w = self.csys.orient * m3d.Vector(velocities[3:])
URRobot.speedx(self, command, np.concatenate((v.array, w.array)), acc,
min_time)
def fit_points(self, points):
"""Compute the line from a set of points. 'points'
must be an array of row position vectors, such that
points[i] is a position vector."""
points = np.array(points)
centre = np.sum(points, axis=0)/len(points)
eigen = np.linalg.eig(np.cov(points.T))
max_ev_i = np.where(eigen[0] == max(eigen[0]))[0][0]
direction = eigen[1].T[max_ev_i]
self._p = m3d.Vector(centre)
self._d = self._ud = m3d.Vector(direction)
def get_laser_to_turntable_transform(self):
self.laser_to_camera_trf = m3d.Transform(np.hstack((self.tvec.reshape(3),
self.rvec.reshape(3))))
tbl_zaxis = m3d.Vector(self.axis).normalized()
tbl_center = m3d.Vector(self.center)
tbl_xaxis = (m3d.Vector.e1 - tbl_zaxis.y * tbl_zaxis).normalized()
self.table_to_camera_trf = m3d.Transform.new_from_xzp(tbl_xaxis,
tbl_zaxis,
tbl_center)
return self.table_to_camera_trf.inverse() * self.laser_to_camera_trf
def set_array(self, array):
"""Set the wrench by six values in the iterable 'array'. The first
three values must specify force, the last three must specify
moment
"""
if len(array) != 6:
raise Exception(
self.__class__.__name__ +
'Setting the value by the "array" property needs exactly'
+ ' six values.({} were given)'.format(len(array)))
self._force = m3d.Vector(array[:3])
self._moment = m3d.Vector(array[3:])
def pn_to_pv(cls, point, normal):
"""Compute the plane vector of a plane represented by a point
and normal."""
if not isinstance(point, m3d.Vector):
point = m3d.Vector(point)
if not isinstance(normal, m3d.Vector):
normal = m3d.Vector(normal)
# // Origo projection on plane
p0 = (point * normal) * normal
# // Square of offset from origo
d2 = p0.length_squared
# // return the plane vector
return p0 / (d2)
def pn_to_pv(self, p, n):
"""Compute the plane vector of a plane represented by a point
and normal."""
if type(p) != m3d.Vector:
p = m3d.Vector(p)
if type(n) != m3d.Vector:
n = m3d.Vector(n)
# // Origo projection on plane
p0 = (p * n) * n
# // Square of offset from origo
d2 = p0.length_squared
# // return the plane vector
return p0 / (d2)
def vertex(self, vertex_index):
"""Return the requested box vertex.
Arguments:
vertex (int): the index of the requested vertex
"""
# Find the vertex coordinates in a box-fixed reference frame
#
# D --------- C
# - -
# - (0,0) -
# - -
# A --------- B
#
vertex_index %= 4
delta_x = float(self.length) / 2
delta_y = float(self.width) / 2
x_vertex = 0
y_vertex = 0
#A
if vertex_index == 0:
x_vertex = -delta_x
y_vertex = -delta_y
#B
elif vertex_index == 1:
x_vertex = +delta_x
y_vertex = -delta_y
#C
elif vertex_index == 2:
x_vertex = +delta_x
y_vertex = +delta_y
#D
else:
x_vertex = -delta_x
y_vertex = +delta_y
# Calculate the vector
vector = m3d.Vector(x_vertex, y_vertex)
# Rotatate the vector by theta
rotation = m3d.Orientation.new_rot_z(self.theta)
rotated_vector = rotation * vector
# Translate the vector
p_c = m3d.Vector(self.center)
transl_vector = p_c + rotated_vector
vector = transl_vector.get_list()
# we need 2D vector
vector.pop()
return vector
def _test():
# Simple test of projected line.
l1 = Line(point=(0, 1, 1), direction=(0, 0.1, 1))
l2 = Line(point=(1, 1, 0.1), direction=(0, 1, 0))
pl = l1.projected_line(l2)
# print('Projected line: {}'.format(str(pl)))
assert((pl-l1.projected_point(pl))
.length < 10 * m3d.utils.eps)
# Test line fitting.
fl = Line.new_fitted_points([[1, 1, 1],
[2, 2, 2],
[3, 3, 3]])
# print('Fitted line: {}'.format(str(fl)))
assert(fl.point == m3d.Vector(2, 2, 2))
assert(1.0 - np.abs(fl.direction * m3d.Vector(1, 1, 1).normalized)
< 10 * m3d.utils.eps)
def _test(n=5, noise=0.005):
"""Test and return the identification errors for a generated set of
poses and applying a given noise to the synthetic ft when
generating poses.
"""
bt = m3d.Vector(0.5, 0.6, 0.7)
ft = m3d.Vector(0.1, 0.05, 0.15)
# Generate random orientation Assume limited ergodicity
bfs = []
for i in range(n):
o = m3d.Orientation.new_euler(np.random.uniform(0, 1, 3))
p = bt - o * (ft + m3d.Vector(np.random.uniform(-noise, noise, 3)))
bfs.append(m3d.Transform(o, p))
pc = PivotCalibrator(bfs)
pc()
return (pc.ft - ft).length, (pc.bt - bt).length
def reset(self):
"""Reset he explosion to run again!"""
self.angles = {}
self.rots = {}
for i in self.root_mesh.get_names():
a = math3d.Vector(self.root_mesh.get_obj_by_name(i).base_pos)
x, y, z = a.x, a.y, a.z
if x == y == z == 0:
x, y, z = misc.randfloat(-1, 1), misc.randfloat(
-1, 1), misc.randfloat(-1, 1)
else:
a = a.normalize()
x, y, z = a.x, a.y, a.z
y += misc.randfloat(1.5, 2.5)
self.angles[i] = x + misc.randfloat(-1, 1), y + misc.randfloat(
-1, 1), z + misc.randfloat(-1, 1)
self.rots[i] = (misc.randfloat(-10, 10), misc.randfloat(-10, 10),
misc.randfloat(-10, 10))
for i in self.root_vals:
self.root_mesh.get_obj_by_name(i).pos = self.root_vals[i][0]
self.root_mesh.get_obj_by_name(i).rotation = self.root_vals[i][1]
self.age = 0
self.dead = False
self.down_delta = 0
def __init__(self, **kwargs):
"""Create a plane representation by one of the following named
arguments:
* 'plane_vector': A normalized plane vector. The normal will
be pointing away from the origo. If kw-argument 'origo_inside'
is given, this will determine the direction of the plane
normal; otherwise origo will be set inside.
* 'pn_pair': An ordered sequence for creating a reference
point and a normal vector. The normal and point are conserved
as they are given.
* 'points': A set of at least three points for fitting a
plane.
* 'coeffs': Four coefficients (a,b,c,d) for the plane equation
ax+by+cz+d=0.
The internal representation is point and normal. If given as a
pn_pair, A boolean, 'origo_inside', is held to decide the
direction of the normal vector, such that the origo of the
defining coordinate system is on the inside when true.
"""
self._origo_inside = kwargs.get('origo_inside', True)
if 'plane_vector' in kwargs:
pv = m3d.Vector(kwargs['plane_vector'])
(self._p, self._n) = self.pv_to_pn(pv)
elif 'pn_pair' in kwargs:
(self._p, self._n) = [m3d.Vector(e) for e in kwargs['pn_pair']]
# # Override a given origo inside.
self._origo_inside = (self._p * self._n) > 0
# Ensure unit normal
#self._n.normalize()
# Make point a 'minimal' point on the plane, i.e. the
# projection of origo in the plane.
self._p = (self._p * self._n) * self._n
elif 'points' in kwargs:
self.fit_plane(kwargs['points'])
elif 'coeffs' in kwargs:
self.coeffs = kwargs['coeffs']
else:
raise Exception(
'Plane.__init__ : Must have either of constructor ' +
'kw-arguments: "plane_vector", "pn_pair", or ' +
'"points". Neither given!')
def from_yz(self, y_vec, z_vec):
"""Reset this orientation to the one that conforms with the
given x and z directions.
"""
if type(y_vec) != m3d.Vector:
y_vec = m3d.Vector(y_vec)
if type(z_vec) != m3d.Vector:
z_vec = m3d.Vector(z_vec)
y_vec = y_vec.normalized
z_vec = z_vec.normalized
if np.abs(y_vec * z_vec) > utils.eps:
print('Warning: Orthonormalizing z_vec on x_vec!')
z_vec -= (y_vec * z_vec) * y_vec
z_vec.normalize()
self._data[:, 0] = y_vec.cross(z_vec)._data
self._data[:, 1] = y_vec._data
self._data[:, 2] = z_vec._data
def paintGL(self):
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glLinkProgram(self._shader_program)
gl.glUseProgram(self._shader_program)
position_location = gl.glGetAttribLocation(self._shader_program,
"position")
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self._vbo)
gl.glEnableVertexAttribArray(position_location)
gl.glVertexAttribPointer(position_location, 3, gl.GL_FLOAT,
gl.GL_FALSE, 0, None)
normal_location = gl.glGetAttribLocation(self._shader_program,
"normal")
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self._normal_vbo)
gl.glEnableVertexAttribArray(normal_location)
gl.glVertexAttribPointer(normal_location, 3, gl.GL_FLOAT, gl.GL_FALSE,
0, None)
model = math3d.Matrix44.translate(0, 0, 0)
view = math3d.Matrix44.look_at_right_hand(math3d.Vector(4, 3, 10),
math3d.Vector(0, 0, 0),
math3d.Vector(0, 1, 0))
projection = math3d.Matrix44.projection_right_hand(
math.radians(45), 4.0 / 3.0, 0.1, 100.0)
mvp = model * view * projection
model_id = gl.glGetUniformLocation(self._shader_program, "model")
gl.glUniformMatrix4fv(model_id, 1, gl.GL_FALSE, model.raw_data())
view_id = gl.glGetUniformLocation(self._shader_program, "view")
gl.glUniformMatrix4fv(view_id, 1, gl.GL_FALSE, view.raw_data())
mvp_id = gl.glGetUniformLocation(self._shader_program, "mvp")
gl.glUniformMatrix4fv(mvp_id, 1, gl.GL_FALSE, mvp.raw_data())
light_position = math3d.Vector(10, 10, 10)
light_position_id = gl.glGetUniformLocation(
self._shader_program, "LightPosition_worldspace")
gl.glUniform3fv(light_position_id, 1, light_position.raw_data())
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self._index_buffer)
gl.glDrawElements(gl.GL_TRIANGLES, len(self._indices),
gl.GL_UNSIGNED_INT, ctypes.c_void_p(0))
def pos_sif(self):
if self._pos_sif is None:
# // d is the vertical stacking of b_A_i - Theta_X*b_B_i
d = np.hstack([(fm.d - self.orient_sif * sm.d)._data
for fm, sm in self._move_pairs])
# // The solution to the position, b_X, is now computed as (C^T*C)^(-1)*C*d
self._pos_sif = m3d.Vector(self.C_pinv.dot(d))
return self._pos_sif
def test_transform(self):
p = hcn.HPose()
t = m3d.Transform()
t.pos = m3d.Vector(2, 3, 1)
t.orient.rotate_zb(math.pi / 2)
p = hcn.HPose(*t.pose_vector)
t2 = m3d.Transform(p.to_list()[:-1])
self.assertEqual(t, t2)
def from_xy(self, x_vec, y_vec):
"""Reset this orientation to the one that conforms with the
given x and y directions.
"""
if type(x_vec) != m3d.Vector:
x_vec = m3d.Vector(x_vec)
if type(y_vec) != m3d.Vector:
y_vec = m3d.Vector(y_vec)
x_vec = x_vec.normalized
y_vec = y_vec.normalized
if np.abs(x_vec * y_vec) > utils.eps:
print('Warning: Orthonormalizing y_vec on x_vec!')
y_vec -= (x_vec * y_vec) * x_vec
y_vec.normalize()
self._data[:, 0] = x_vec._data
self._data[:, 1] = y_vec._data
self._data[:, 2] = x_vec.cross(y_vec)._data