def solve_system(code,A,B,x,y,z):
'''
solves a trigonometric system of equations
x = cos(t0)*(A*sin(t1) + B*cos(t1)) (1)
y = sin(t0)*(A*sin(t1) + B*cos(t1)) (2)
z = -B*sin(t1) + A*cos(t1) (3)
for two angles t0 and t1 and returns a set of paired solutions as tulips (t0,t1)
The system is over constrained so a solution may not exist. In thic case, an empty set is returned.
'''
solution_set = []
t0 = []
t0.append(math.atan2(y, x))
t0.append(t0[0] + math.pi)
t1 = solve_equation(1, [A, - B, - z])
for i in range(0,len(t0)):
for j in range(0,len(t1)):
q = A*math.sin(t1[j]) + B*math.cos(t1[j])
p = A*math.cos(t1[j]) - B*math.sin(t1[j])
c0 = math.cos(t0[i])
s0 = math.sin(t0[i])
if (gen.equal(x , c0*q) and
gen.equal(y , s0*q) and
gen.equal(z , p)):
solution_set.append((t0[i],t1[j]))
return solution_set
def unit_quaternion(TRM):
'''
Returns a vector (4 X 1) containing elements of the unit quaternion corresponding to transformation or rotation matrix TRM.
(TRM can be the 4*4 transformation matrix or 3*3 rotation matrix)
the first element of the output vector contains the scalar part of the unit quaternion and the last three elements represent the vectorial part
'''
assert gen.equal(numpy.linalg.det(TRM[0:3, 0:3]), 1.0), genpy.err_str(
__name__, self.__class__.__name__,
sys._getframe().f_code.co_name, "Given TRM is not a rotation matrix.")
uqn = numpy.zeros((4))
uqn[0] = 1.00
[u, v, w] = permutation_uvw(TRM)
p = 1.0 + TRM[u, u] - TRM[v, v] - TRM[w, w]
if (p > gen.epsilon):
p = math.sqrt(p)
uqn[u + 1] = p / 2.0
uqn[v + 1] = (TRM[v, u] + TRM[u, v]) / (2 * p)
uqn[w + 1] = (TRM[w, u] + TRM[u, w]) / (2 * p)
uqn[0] = (TRM[w, v] - TRM[v, w]) / (2 * p)
if uqn[0] < 0:
uqn = -uqn
assert gen.equal(numpy.linalg.norm(uqn), 1.0), "Impossible !"
return uqn
def solve_system(code, A, B, x, y, z):
'''
solves a trigonometric system of equations
x = cos(t0)*(A*sin(t1) + B*cos(t1)) (1)
y = sin(t0)*(A*sin(t1) + B*cos(t1)) (2)
z = -B*sin(t1) + A*cos(t1) (3)
for two angles t0 and t1 and returns a set of paired solutions as tulips (t0,t1)
The system is over constrained so a solution may not exist. In thic case, an empty set is returned.
'''
solution_set = []
t0 = []
t0.append(math.atan2(y, x))
t0.append(t0[0] + math.pi)
t1 = solve_equation(1, [A, -B, -z])
for i in range(0, len(t0)):
for j in range(0, len(t1)):
q = A * math.sin(t1[j]) + B * math.cos(t1[j])
p = A * math.cos(t1[j]) - B * math.sin(t1[j])
c0 = math.cos(t0[i])
s0 = math.sin(t0[i])
if (gen.equal(x, c0 * q) and gen.equal(y, s0 * q)
and gen.equal(z, p)):
solution_set.append((t0[i], t1[j]))
return solution_set
def writing_posture(self):
if self.larm_startpoint == None:
print "Left Arm is not calibrated!"
else:
pos = self.larm_startpoint
if (self.larm_endpoint_width == None) or (self.larm_endpoint_height
== None):
ori = ps.rot.point_forward_orientation
else:
z = self.larm_endpoint_height - self.larm_startpoint
self.box_height_larm = np.linalg.norm(z)
z = z / self.box_height_larm
y = self.larm_endpoint_width - self.larm_startpoint
if y[1] < 0:
y = y / self.box_width_larm
else:
y = -y / self.box_width_larm
self.box_width_larm = np.linalg.norm(y)
y = -y / self.box_width_larm
x = np.cross(y, z)
x = x / np.linalg.norm(x)
# Correct z:
z = np.cross(x, y)
ori = np.append(np.append([y], [z], axis=0), [x], axis=0).T
assert gen.equal(np.linalg.det(ori), 1.0)
n = ori[:, 2]
self.larm.set_target(pos - self.depth * n, ori)
if self.rarm_startpoint == None:
print "Right Arm is not calibrated!"
else:
pos = self.rarm_startpoint
if (self.rarm_endpoint_width == None) or (self.rarm_endpoint_height
== None):
ori = ps.rot.point_forward_orientation
else:
z = self.rarm_endpoint_height - self.rarm_startpoint
self.box_height_rarm = np.linalg.norm(z)
z = z / self.box_height_rarm
y = self.rarm_endpoint_width - self.rarm_startpoint
self.box_width_rarm = np.linalg.norm(y)
if y[1] < 0:
y = -y / self.box_width_rarm
else:
y = y / self.box_width_rarm
x = np.cross(y, z)
x = x / np.linalg.norm(x)
# Correct z:
z = np.cross(x, y)
ori = np.append(np.append([y], [z], axis=0), [x], axis=0).T
assert gen.equal(np.linalg.det(ori), 1.0)
n = ori[:, 2]
self.rarm.set_target(pos - self.depth * n, ori)
self.larm_target(wait=False)
self.rarm_target(wait=False)
pint.wait_until_finished(target_list=['rarm', 'larm'])
self.sync_object()
'''
def writing_posture(self):
if self.larm_startpoint == None:
print "Left Arm is not calibrated!"
else:
pos = self.larm_startpoint
if (self.larm_endpoint_width == None) or (self.larm_endpoint_height == None):
ori = ps.rot.point_forward_orientation
else:
z = self.larm_endpoint_height - self.larm_startpoint
self.box_height_larm = np.linalg.norm(z)
z = z/self.box_height_larm
y = self.larm_endpoint_width - self.larm_startpoint
if y[1] < 0:
y = y/self.box_width_larm
else:
y = - y/self.box_width_larm
self.box_width_larm = np.linalg.norm(y)
y = - y/self.box_width_larm
x = np.cross(y, z)
x = x/np.linalg.norm(x)
# Correct z:
z = np.cross(x, y)
ori = np.append(np.append([y],[z],axis = 0), [x], axis = 0).T
assert gen.equal(np.linalg.det(ori), 1.0)
n = ori[:,2]
self.larm.set_target(pos - self.depth*n, ori)
if self.rarm_startpoint == None:
print "Right Arm is not calibrated!"
else:
pos = self.rarm_startpoint
if (self.rarm_endpoint_width == None) or (self.rarm_endpoint_height == None):
ori = ps.rot.point_forward_orientation
else:
z = self.rarm_endpoint_height - self.rarm_startpoint
self.box_height_rarm = np.linalg.norm(z)
z = z/self.box_height_rarm
y = self.rarm_endpoint_width - self.rarm_startpoint
self.box_width_rarm = np.linalg.norm(y)
if y[1] < 0:
y = - y/self.box_width_rarm
else:
y = y/self.box_width_rarm
x = np.cross(y, z)
x = x/np.linalg.norm(x)
# Correct z:
z = np.cross(x, y)
ori = np.append(np.append([y],[z],axis = 0), [x], axis = 0).T
assert gen.equal(np.linalg.det(ori), 1.0)
n = ori[:,2]
self.rarm.set_target(pos-self.depth*n, ori)
self.larm_target(wait = False)
self.rarm_target(wait = False)
pint.wait_until_finished(target_list = ['rarm', 'larm'])
self.sync_object()
'''
def __init__(self, M = np.eye(3), center = Point_3D(pos = np.zeros(3))):
'''
For example to have a non-rotated ellipsoid centered at the origin with equation:
x^2/a^2 + y^2/b^2 + z^2/c^2 = 1
Arguments M and center must be selected as:
[ a^(-2) 0 0 ]
M = [ 0 b^(-2) 0 ]
[ 0 0 c^(-2) ]
center = (0, 0, 0)
'''
# Checking arguments:
func_name = sys._getframe().f_code.co_name
genpy.check_type(M, [np.ndarray], __name__, self.__class__.__name__, func_name, 'M', shape = (3,3))
(landa, V) = np.linalg.eig(M)
# V is a rotation that transforms from principal coordinate system to the actual coordinate system
# if x is a vector in principal CS and x' is its representation in actual CS: x' = V*x and x = V^T*x'
assert vm.positive(landa, non_zero = True) and gen.equal(np.linalg.norm(V.imag) , 0.0), genpy.err_str(__name__, self.__class__.__name__, func_name, 'Matrix M must have real and positive eigen values')
#
self.M = M
self.center = center
self.a = math.sqrt(1.0/landa[0])
self.b = math.sqrt(1.0/landa[1])
self.c = math.sqrt(1.0/landa[2])
self.R = V.real
def intersection(self, line):
M = np.array([[self.a, self.b],[line.a, line.b]])
c = np.array([- self.c, - line.c])
if gen.equal(np.linalg.det(M), 0.0):
return None
else:
return Point_2D(np.dot(np.linalg.inv(M), c))
def ts_project(self, js_traj, phi_start = 0.0, phi_end = None):
'''
projects the given jointspace trajectory into the taskspace
The phase starts from phi_start and added by delta_phi in each step.
if at any time the joint values are not feasible, the process is stopped.
'''
if phi_end == None:
phi_end = js_traj.phi_end
pt = trajlib.Trajectory_Polynomial(dimension = 3*len(self.task_point) + 9*len(self.task_frame))
if phi_end > js_traj.phi_end:
phi_end = js_traj.phi_end
phi = phi_start
stay = True
while stay:
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
js_traj.set_phi(phi)
if self.set_config(js_traj.current_position):
pt.add_point(phi - phi_start, self.pose())
phi = phi + self.ik_settings.time_step
return pt
def intersection(self, line):
M = np.array([[self.a, self.b], [line.a, line.b]])
c = np.array([-self.c, -line.c])
if gen.equal(np.linalg.det(M), 0.0):
return None
else:
return Point_2D(np.dot(np.linalg.inv(M), c))
def ts_project(self, js_traj, phi_start = 0.0, phi_end = None):
'''
projects the given jointspace trajectory into the taskspace
The phase starts from phi_start and added by delta_phi in each step.
if at any time the joint values are not feasible, the process is stopped.
'''
if phi_end == None:
phi_end = js_traj.phi_end
pt = trajlib.Trajectory_Polynomial(dimension = 3*len(self.task_point) + 9*len(self.task_frame))
if phi_end > js_traj.phi_end:
phi_end = js_traj.phi_end
phi = phi_start
stay = True
while stay:
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
js_traj.set_phi(phi)
if self.set_config(js_traj.current_position):
pt.add_point(phi - phi_start, self.pose())
phi = phi + self.ik_settings.time_step
return pt
def __init__(self, M=np.eye(3), center=Point_3D(pos=np.zeros(3))):
'''
For example to have a non-rotated ellipsoid centered at the origin with equation:
x^2/a^2 + y^2/b^2 + z^2/c^2 = 1
Arguments M and center must be selected as:
[ a^(-2) 0 0 ]
M = [ 0 b^(-2) 0 ]
[ 0 0 c^(-2) ]
center = (0, 0, 0)
'''
# Checking arguments:
func_name = sys._getframe().f_code.co_name
genpy.check_type(M, [np.ndarray],
__name__,
self.__class__.__name__,
func_name,
'M',
shape=(3, 3))
(landa, V) = np.linalg.eig(M)
# V is a rotation that transforms from principal coordinate system to the actual coordinate system
# if x is a vector in principal CS and x' is its representation in actual CS: x' = V*x and x = V^T*x'
assert vm.positive(landa, non_zero=True) and gen.equal(
np.linalg.norm(V.imag), 0.0), genpy.err_str(
__name__, self.__class__.__name__, func_name,
'Matrix M must have real and positive eigen values')
#
self.M = M
self.center = center
self.a = math.sqrt(1.0 / landa[0])
self.b = math.sqrt(1.0 / landa[1])
self.c = math.sqrt(1.0 / landa[2])
self.R = V.real
def arc(traj=None,
center_dw=0.1,
center_dh=0.0,
direction=None,
angle=360.0,
d_theta=10.0,
clockwise=True,
static_ends=True,
in_degrees=True):
'''
adds a set of points to the given trajectory establishing an arc in a plane parallel to the yz plane starting from initial_pos,
The points follow an arc with given angle around the center
input trj must be a Trajectory_Polynomial
if trj is not given, a new trajectory will be created
'''
if in_degrees:
angle = angle * math.pi / 180.0
d_theta = d_theta * math.pi / 180.0
(trj, w, h, n, v) = decode_arguments(traj, direction, static_ends)
if clockwise:
sgn = 1
else:
sgn = -1
center = center_dw * w + center_dh * h
nseg = len(trj.segment)
npnt = len(trj.segment[nseg - 1].point)
initial_pos = np.copy(trj.segment[nseg - 1].point[npnt - 1].pos)
initial_phi = trj.phi_end
r = math.sqrt(center_dw**2 + center_dh**2)
t0 = math.acos(-center_dw / r)
t = 0.0
stay = True
pos = np.zeros(3)
while stay:
t = t + d_theta
if t > angle:
t = angle
pos = initial_pos + center + r * math.cos(
sgn * t + t0) * w + r * math.sin(sgn * t + t0) * h
vel = -r * sgn * math.sin(sgn * t + t0) * w + r * sgn * math.cos(
sgn * t + t0) * h
# trj.new_segment()
trj.add_point(phi=initial_phi + t, pos=np.copy(pos), vel=vel)
if gen.equal(t, angle):
stay = False
lsi = len(trj.segment) - 1
lpi = len(trj.segment[lsi].point) - 1
trj.segment[lsi].point[lpi].vel = v
return trj
def positive(v, non_zero = False):
permit = True
n = len(v)
i = 0
while permit and (i < n):
permit = permit and (v[i] >= 0) and (not (non_zero and gen.equal(v[i], 0.0)))
i += 1
return permit
def normalize(v):
'''
Returns the unit vector parallel to the given vector.
'''
l = np.linalg.norm(v)
if gen.equal(l, 0.0):
return(v)
else:
return(v/l)
def symbolic_term(a, x, plus_sign = True):
if plus_sign and (a > 0):
ps = '+ '
else:
ps = ''
if x == '':
star = ''
else:
star = '*'
if gen.equal(a , 0.0):
s = ''
elif gen.equal(a , 1.0) and (star == '*'):
s = ps + x + ' '
elif gen.equal(a , - 1.0) and (star == '*'):
s = '- ' + x + ' '
else:
s = ps + str(a) + star + x
return s
def symbolic_term(a, x, plus_sign=True):
if plus_sign and (a > 0):
ps = '+ '
else:
ps = ''
if x == '':
star = ''
else:
star = '*'
if gen.equal(a, 0.0):
s = ''
elif gen.equal(a, 1.0) and (star == '*'):
s = ps + x + ' '
elif gen.equal(a, -1.0) and (star == '*'):
s = '- ' + x + ' '
else:
s = ps + str(a) + star + x
return s
def zeta(self, phi = None):
if phi == None:
phi = self.angle()
if gen.equal(phi, 0.0) and self.parametrization == 'identity':
return self.m
p = self.gen_fun(phi)
return 2*math.tan(phi/2)*gen.inv(p)
def normalize(v):
'''
Returns the unit vector parallel to the given vector.
'''
l = np.linalg.norm(v)
if gen.equal(l, 0.0):
return (v)
else:
return (v / l)
def positive(v, non_zero=False):
permit = True
n = len(v)
i = 0
while permit and (i < n):
permit = permit and (v[i] >= 0) and (not (non_zero
and gen.equal(v[i], 0.0)))
i += 1
return permit
def zeta(self, phi=None):
if phi == None:
phi = self.angle()
if gen.equal(phi, 0.0) and self.parametrization == 'identity':
return self.m
p = self.gen_fun(phi)
return 2 * math.tan(phi / 2) * gen.inv(p)
def __init__(self,
a=Point_2D(np.zeros(2)),
b=Point_2D([1.0, 1.0]),
c=None,
representation='points'):
'''
Equation of the line in 2D space:
a*x + b*y + c = 0
'''
if representation == 'points':
[x1, y1] = a.cartesian()
[x2, y2] = b.cartesian()
assert (not gen.equal(x1, x2)) or (not gen.equal(
y1, y2)), "The two given points are identical"
self.a = y2 - y1
self.b = x1 - x2
self.c = -x2 * self.a - y2 * self.b
elif representation == 'point-angle':
[x0, y0] = a.cartesian()
if gen.equal(math.cos(b), 0.0):
self.a = 1.0
self.b = 0.0
self.c = -x0
else:
self.a = math.tan(b)
self.b = -1.0
self.c = y0 - self.a * x0
elif representation == 'equation':
self.a = a
self.b = b
self.c = c
else:
assert False, "Unknown representation"
self.theta = math.atan2(-self.a, self.b)
if gen.equal(self.b, 0.0): # Equation: x = h
self.m = np.inf
self.h = -self.c / self.a
else: # Equation: y = m*x + h
self.m = -self.a / self.b
self.h = -self.c / self.b
def arcsincos(s, c):
'''
return the angle that its sine is "s" and its cosine is "c"
'''
assert gen.equal(s**2 + c**2, 1.0)
if s < 0:
theta = -arccos(c)
else:
theta = arccos(c)
return theta
def arcsincos(s, c):
'''
return the angle that its sine is "s" and its cosine is "c"
'''
assert gen.equal(s**2 + c**2, 1.0)
if s < 0:
theta = - arccos(c)
else:
theta = arccos(c)
return theta
def js_create(self, phi_end = None, traj_capacity = 2, traj_type = 'regular'):
'''
'''
self.damping_factor = self.ik_settings.initial_damping_factor
if self.ik_settings.generate_log:
self.run_log.clear()
keep_q = np.copy(self.q)
H = self.transfer_matrices()
if phi_end == None:
phi_end = self.ik_settings.number_of_steps*self.ik_settings.time_step
if traj_type == 'regular':
jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity)
elif traj_type == 'polynomial':
jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity)
else:
assert False, "\n Unknown Trajectory Type \n"
jt.plot_settings.axis_label = self.config_settings.joint_label
if self.ik_settings.respect_limits_in_trajectories:
jt.vel_max = self.ik_settings.max_js
jt.acc_max = self.ik_settings.max_ja
jt.pos_max = self.config_settings.qh
jt.pos_min = self.config_settings.ql
phi = 0.0
jt.add_position(0.0, pos = np.copy(self.q))
stay = not self.in_target()
while stay:
stay = not self.in_target()
phi = phi + self.ik_settings.time_step
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
err_reduced = self.moveto_target()
if err_reduced or (not self.ik_settings.respect_error_reduction):
jt.add_position(phi = phi, pos = np.copy(self.q))
if traj_type == 'polynomial':
jt.interpolate()
self.set_config(keep_q)
if self.ik_settings.generate_log:
self.run_log.time_elapsed = time.time() - self.run_log.time_start
return jt
def solve_equation(code, parameters):
'''
Solves the trigonometric equation for t denoted by:
code = 1: parameters should be: [p1, p2, p3]
p1*cos(t) + p2*sin(t) + p3 = 0
code = 2:
p1*cos^2(t) + p2*sin^2(t) + p3*sin(t)*cos(t) + p4*sin(t) + p5*cos(t) + p6 = 0
It returns the solution set as an array
If no solution exists, it returns an empty array
'''
solution_set = []
if code == 1:
R = math.sqrt(parameters[0]**2 + parameters[1]**2)
assert (not gen.equal(R, 0))
s = -parameters[2] / R
if in_domain_sin(s):
sai0 = math.atan2(parameters[0], parameters[1])
if gen.equal(math.sin(sai0), parameters[0] / R):
sai = sai0
elif equal(math.sin(sai0), -parameters[0] / R):
sai = sai0 + math.pi
else:
# If you came here, something is wrong !
assert (False)
t1 = arcsin(s) - sai
t2 = math.pi - t1 - 2 * sai
if gen.equal(
parameters[0] * math.cos(t1) +
parameters[1] * math.sin(t1) + parameters[2], 0):
solution_set.append(t1)
if gen.equal(
parameters[0] * math.cos(t2) +
parameters[1] * math.sin(t2) + parameters[2], 0):
solution_set.append(t2)
return solution_set
def __init__(self, a = Point_2D(np.zeros(2)), b = Point_2D([1.0, 1.0]), c = None, representation = 'points'):
'''
Equation of the line in 2D space:
a*x + b*y + c = 0
'''
if representation == 'points':
[x1, y1] = a.cartesian()
[x2, y2] = b.cartesian()
assert (not gen.equal(x1, x2)) or (not gen.equal(y1, y2)), "The two given points are identical"
self.a = y2 - y1
self.b = x1 - x2
self.c = - x2*self.a - y2*self.b
elif representation == 'point-angle':
[x0, y0] = a.cartesian()
if gen.equal(math.cos(b), 0.0):
self.a = 1.0
self.b = 0.0
self.c = - x0
else:
self.a = math.tan(b)
self.b = - 1.0
self.c = y0 - self.a*x0
elif representation == 'equation':
self.a = a
self.b = b
self.c = c
else:
assert False, "Unknown representation"
self.theta = math.atan2(- self.a, self.b)
if gen.equal(self.b, 0.0): # Equation: x = h
self.m = np.inf
self.h = - self.c/ self.a
else: # Equation: y = m*x + h
self.m = - self.a / self.b
self.h = - self.c / self.b
def feasible_stepsize(direction, x, x_min, x_max):
'''
when the correction of vector x is restricted by limits
If you want to change vector x in a desired direction, and there are lower and upper bounds for x,
how much are you allowed to change?
This function returns a feasible stepsize for the given direction.
The direction of change must be multiplied by a scalar value lower or equal to this feasible stepsize
so that the applied changes are feasible.
'''
valtyp = [np.ndarray, list, tuple]
direction = genpy.check_type(direction,
valtyp,
__name__,
function_name=sys._getframe().f_code.co_name,
var_name='direction',
shape_length=1)
n = len(direction)
[x, x_min,
x_max] = genpy.check_types(variables=[x, x_min, x_max],
type_lists=[valtyp, valtyp, valtyp],
file_path=__name__,
function_name=sys._getframe().f_code.co_name,
var_names=['x', 'x_min', 'x_max'],
shape_lengths=[1, 1, 1],
array_lengths=[n, n, n])
etta = []
for i in range(n):
if not gen.equal(direction[i], 0.0, epsilon=0.000000001):
if x_min[i] == None:
x_min[i] = -np.inf
if x_max[i] == None:
x_max[i] = np.inf
a = (x_min[i] - x[i]) / direction[i]
b = (x_max[i] - x[i]) / direction[i]
etta.append(gen.sign_choice(b, a, direction[i]))
else:
etta.append(np.inf)
if etta[len(etta) - 1] < 0:
print 'x_min: ', x_min
print 'x_max: ', x_max
print 'x: ', x
print 'dir: ', direction
if etta == []:
return 0.0
else:
return min(etta)
def intersection(self, line):
'''
a*x+b*y+c=0
y = m*x + h
(x - x0)^2 + (m*x + h - y0)^2 = R^2
(1+m^2)*x^2 + 2*[m*(h-y0) - x0]*x + (h - y0)^2 - R^2 = 0
'''
[x0, y0] = self.center.cartesian()
sol = []
h_y0 = line.h - y0
alpha = 1.0 + line.m*line.m
if line.m == np.inf:
delta = self.R*self.R - (x0 - line.h)**2
if gen.equal(delta , 0.0):
sol.append(Point_2D(np.array([xp,y0])))
elif delta > 0:
sd = math.sqrt(delta)
sol.append(Point_2D(np.array([line.h, y0 - sd])))
sol.append(Point_2D(np.array([line.h, y0 + sd])))
else:
betta = line.m*h_y0 - x0
gamma = x0*x0 + h_y0*h_y0 - self.R*self.R
delta = betta*betta - alpha*gamma
if gen.equal(delta , 0.0):
x = - betta / alpha
y = line.m*x + line.h
sol.append(Point_2D(np.array([x,y])))
elif delta > 0:
sd = math.sqrt(delta)
x1 = (- betta - sd)/alpha
y1 = line.m*x1 + line.h
sol.append(Point_2D(np.array([x1,y1])))
x2 = (- betta + sd)/alpha
y2 = line.m*x2 + line.h
sol.append(Point_2D(np.array([x2,y2])))
return sol
def solve_equation(code, parameters):
'''
Solves the trigonometric equation for t denoted by:
code = 1: parameters should be: [p1, p2, p3]
p1*cos(t) + p2*sin(t) + p3 = 0
code = 2:
p1*cos^2(t) + p2*sin^2(t) + p3*sin(t)*cos(t) + p4*sin(t) + p5*cos(t) + p6 = 0
It returns the solution set as an array
If no solution exists, it returns an empty array
'''
solution_set = []
if code == 1:
R = math.sqrt(parameters[0]**2 + parameters[1]**2)
assert (not gen.equal(R, 0))
s = - parameters[2] / R
if in_domain_sin(s):
sai0 = math.atan2(parameters[0], parameters[1])
if gen.equal(math.sin(sai0), parameters[0]/R):
sai = sai0
elif equal(math.sin(sai0), - parameters[0]/R):
sai = sai0 + math.pi
else:
# If you came here, something is wrong !
assert(False)
t1 = arcsin(s) - sai
t2 = math.pi - t1 - 2*sai
if gen.equal(parameters[0]*math.cos(t1) + parameters[1]*math.sin(t1) + parameters[2] , 0):
solution_set.append(t1)
if gen.equal(parameters[0]*math.cos(t2) + parameters[1]*math.sin(t2) + parameters[2] , 0):
solution_set.append(t2)
return solution_set
def intersection(self, line):
'''
a*x+b*y+c=0
y = m*x + h
(x - x0)^2 + (m*x + h - y0)^2 = R^2
(1+m^2)*x^2 + 2*[m*(h-y0) - x0]*x + (h - y0)^2 - R^2 = 0
'''
[x0, y0] = self.center.cartesian()
sol = []
h_y0 = line.h - y0
alpha = 1.0 + line.m * line.m
if line.m == np.inf:
delta = self.R * self.R - (x0 - line.h)**2
if gen.equal(delta, 0.0):
sol.append(Point_2D(np.array([xp, y0])))
elif delta > 0:
sd = math.sqrt(delta)
sol.append(Point_2D(np.array([line.h, y0 - sd])))
sol.append(Point_2D(np.array([line.h, y0 + sd])))
else:
betta = line.m * h_y0 - x0
gamma = x0 * x0 + h_y0 * h_y0 - self.R * self.R
delta = betta * betta - alpha * gamma
if gen.equal(delta, 0.0):
x = -betta / alpha
y = line.m * x + line.h
sol.append(Point_2D(np.array([x, y])))
elif delta > 0:
sd = math.sqrt(delta)
x1 = (-betta - sd) / alpha
y1 = line.m * x1 + line.h
sol.append(Point_2D(np.array([x1, y1])))
x2 = (-betta + sd) / alpha
y2 = line.m * x2 + line.h
sol.append(Point_2D(np.array([x2, y2])))
return sol
def tensor(self):
if self.H == None:
if gen.equal(self.angle(), 0.0) :
return np.eye(3)
p = self.vector()
px = rot.skew(p)
px2 = np.dot(px, px)
p_nrm = np.linalg.norm(p)
p2 = p_nrm*p_nrm
p2_inv = gen.inv(p2)
v = self.noo()
z = self.zeta()
m = self.moo()
v2 = v*v
v2_z = v2*gen.inv(z)
self.H = m*np.eye(3) + 0.5*v2*px + p2_inv*(m - v2_z)*px2
return self.H
def tensor(self):
if self.H == None:
if gen.equal(self.angle(), 0.0):
return np.eye(3)
p = self.vector()
px = rot.skew(p)
px2 = np.dot(px, px)
p_nrm = np.linalg.norm(p)
p2 = p_nrm * p_nrm
p2_inv = gen.inv(p2)
v = self.noo()
z = self.zeta()
m = self.moo()
v2 = v * v
v2_z = v2 * gen.inv(z)
self.H = m * np.eye(3) + 0.5 * v2 * px + p2_inv * (m - v2_z) * px2
return self.H
def draw_shape(self, shape_trajectory):# add wait argument later
arm = self.reference_arm()
keep_dt = arm.dt
arm.dt = shape_trajectory.phi_end/100
jt = arm.js_project(shape_trajectory, relative = False, traj_type = 'polynomial')
arm.dt = keep_dt
# jt.fix_points()
jt.consistent_velocities()
# jt.plot()
if not gen.equal(jt.phi_end, shape_trajectory.phi_end):
print "Warning from PyRide_PR2.draw_shape(): All the shape is not in the workspace. Part of it will be drawn !"
L = sum(shape_trajectory.points_dist())
'''
shape_trajectory.plot3d()
for j in range(7):
jt.plot(axis = j, show_points = True)
'''
pint.run_config_trajectory(jt, duration = L/self.arm_speed, is_left_arm = self.larm_reference)
def joint_stepsize_interval(self, direction, max_speed = gen.infinity, delta_t = 0.001):
etta_l = []
etta_h = []
# for ii in range(self.config_settings.njoint):
for i in range(self.config_settings.DOF):
if not gen.equal(direction[i], 0.0):
if self.config_settings.limited[i] and self.config_settings.joint_handling[i] == 'NM':
a = (self.ql[i] - self.q[i])/direction[i]
b = (self.qh[i] - self.q[i])/direction[i]
etta_l.append(gen.sign_choice(a, b, direction[i]))
etta_h.append(gen.sign_choice(b, a, direction[i]))
a = delta_t*max_speed/direction[i]
etta_l.append(gen.sign_choice(-a, a, direction[i]))
etta_h.append(gen.sign_choice( a,-a, direction[i]))
if (etta_l == []) or (etta_h == []):
return (0.0, 0.0)
else:
return (max(etta_l), min(etta_h))
def feasible_stepsize(direction, x, x_min, x_max):
'''
when the correction of vector x is restricted by limits
If you want to change vector x in a desired direction, and there are lower and upper bounds for x,
how much are you allowed to change?
This function returns a feasible stepsize for the given direction.
The direction of change must be multiplied by a scalar value lower or equal to this feasible stepsize
so that the applied changes are feasible.
'''
valtyp = [np.ndarray, list, tuple]
direction = genpy.check_type(direction, valtyp, __name__, function_name = sys._getframe().f_code.co_name, var_name = 'direction', shape_length = 1)
n = len(direction)
[x, x_min, x_max] = genpy.check_types(variables = [x,x_min,x_max], type_lists = [valtyp,valtyp,valtyp],
file_path = __name__, function_name = sys._getframe().f_code.co_name,
var_names = ['x', 'x_min', 'x_max'], shape_lengths = [1,1,1], array_lengths = [n,n,n])
etta = []
for i in range(n):
if not gen.equal(direction[i], 0.0, epsilon = 0.000000001):
if x_min[i] == None:
x_min[i] = - np.inf
if x_max[i] == None:
x_max[i] = np.inf
a = (x_min[i] - x[i])/direction[i]
b = (x_max[i] - x[i])/direction[i]
etta.append(gen.sign_choice(b, a, direction[i]))
else:
etta.append(np.inf)
if etta[len(etta)-1] < 0:
print 'x_min: ', x_min
print 'x_max: ', x_max
print 'x: ', x
print 'dir: ', direction
if etta == []:
return 0.0
else:
return min(etta)
def draw_shape(self, shape_trajectory): # add wait argument later
arm = self.reference_arm()
keep_dt = arm.dt
arm.dt = shape_trajectory.phi_end / 100
jt = arm.js_project(shape_trajectory,
relative=False,
traj_type='polynomial')
arm.dt = keep_dt
# jt.fix_points()
jt.consistent_velocities()
# jt.plot()
if not gen.equal(jt.phi_end, shape_trajectory.phi_end):
print "Warning from PyRide_PR2.draw_shape(): All the shape is not in the workspace. Part of it will be drawn !"
L = sum(shape_trajectory.points_dist())
'''
shape_trajectory.plot3d()
for j in range(7):
jt.plot(axis = j, show_points = True)
'''
pint.run_config_trajectory(jt,
duration=L / self.arm_speed,
is_left_arm=self.larm_reference)
def run_config_trajectory(j_traj, duration = 10.0, dt = None, phi_dot = None, is_left_arm = False):
global rarm_reached, rarm_failed
global larm_reached, larm_failed
if is_left_arm:
larm_failed = False
larm_reached = False
else:
rarm_failed = False
rarm_reached = False
phi = 0.0
dic_list = []
t = 0.0
if phi_dot == None:
phi_dot = j_traj.phi_end/duration
if dt == None:
dt = 0.1*duration
stay = True
while stay:
if (t > duration) or gen.equal(t, duration, epsilon = 0.1*dt):
t = duration
stay = False
j_traj.set_phi(t*phi_dot)
if is_left_arm:
g = gen_larm_joint_posvel_dict(j_traj.current_position, j_traj.current_velocity*phi_dot, dt)
else:
g = gen_rarm_joint_posvel_dict(j_traj.current_position, j_traj.current_velocity*phi_dot, dt)
dic_list.append(g)
t = t + dt
PyPR2.moveArmWithJointTrajectoryAndSpeed(dic_list)
def distance_from_ellipsoid(a, b, c, x0, y0, z0, t0 = None, silent = True, maximum = False):
global max_iter
'''
Find theta and phi that minimize function:
f(theta, phi) = ( a*cos(theta)*cos(phi) - x0 )^2 + ( b*sin(theta)*cos(phi) - y0 )^2 + ( c*sin(phi) - z0 )^2
The method is numerical using the Hessian matrix.
The iterations start from theta_0 and phi_0
'''
a2 = a*a
b2 = b*b
c2 = c*c
if t0 == None:
if gen.equal(x0, 0.0) and gen.equal(y0, 0.0) and gen.equal(z0, 0.0):
if maximum:
if max(a,b,c) == a: # To maximize the function, min must be replaced by max
t0 = np.zeros(2)
elif max(a,b,c) == b:
t0 = np.array([math.pi/2, 0.0])
else:
t0 = np.array([0.0, math.pi/2])
else:
if min(a,b,c) == a: # To maximize the function, min must be replaced by max
t0 = np.zeros(2)
elif min(a,b,c) == b:
t0 = np.array([math.pi/2, 0.0])
else:
t0 = np.array([0.0, math.pi/2])
else:
if maximum:
ssign = - 1.0
else:
ssign = 1.0
s = ssign/math.sqrt(x0*x0/a2 + y0*y0/b2 + z0*z0/c2) # To find the max distance s must be negative s = -1.0/math.sqrt(...)
the = math.atan2(a*y0, b*x0)
phi = math.atan2(z0*s/c, s*math.sqrt(x0*x0/a2 + y0*y0/b2))
assert gen.equal(a*math.cos(the)*math.cos(phi), x0*s)
assert gen.equal(b*math.sin(the)*math.cos(phi), y0*s)
assert gen.equal(c*math.sin(phi), z0*s)
t0 = np.array([the,phi])
H = np.zeros((2,2))
df = np.zeros(2)
t = t0
stay = True
cnt = 0
while stay and (cnt < max_iter):
ct = math.cos(t[0])
cf = math.cos(t[1])
st = math.sin(t[0])
sf = math.sin(t[1])
ct2 = ct*ct
st2 = st*st
cf2 = cf*cf
sf2 = sf*sf
c2t = ct2 - st2
c2f = cf2 - sf2
s2t = 2*st*ct
s2f = 2*sf*cf
df[0] = 0.5*(b2 - a2)*s2t*cf2 + cf*(a*x0*st - b*y0*ct)
df[1] = - 0.5*s2f*(a2*ct2 + b2*st2 - c2) + sf*(a*x0*ct + b*y0*st) - c*z0*cf
stay = not gen.equal(np.linalg.norm(df), 0.0)
if stay:
H[0,0] = (b2 - a2)*c2t*cf2 + cf*(a*x0*ct + b*y0*st)
H[0,1] = 0.5*(a2 - b2)*s2t*s2f - sf*(a*x0*st - b*y0*ct)
H[1,0] = H[0,1]
H[1,1] = - c2f*(a2*ct2 + b2*st2 - c2) + cf*(a*x0*ct + b*y0*st) + c*z0*sf
t = t - np.dot(np.linalg.inv(H), df)
cnt += 1
if not silent:
print
print 'iteration:', cnt, ' cost function value:', (a*ct*cf - x0)**2 + (b*st*cf - y0)**2 + (c*sf - z0)**2, ' norm(div_f):', np.linalg.norm(df)
return (t[0], t[1])
def possess(self, point):
x = point.cartesian()
return gen.equal(self.a * x[0] + self.b * x[1] + self.c, 0.0)
def js_project(self, pos_traj, ori_traj = None, phi_start = 0.0, phi_end = None, relative = True, traj_capacity = 2, traj_type = 'regular'):
'''
projects the given taskspace pose trajectory into the jointspace using the numeric inverse kinematics.
The phase starts from phi_start and added by self.ik_settings.time_step in each step.
at any time, if a solution is not found, the target is ignored and no configuration is added to the trajectory
'''
self.damping_factor = self.ik_settings.initial_damping_factor
keep_q = np.copy(self.q)
H = self.transfer_matrices()
if ori_traj == None:
ori_traj = trajlib.Orientation_Path()
ori_traj.add_point(0.0, self.task_frame[0].orientation(H))
ori_traj.add_point(pos_traj.phi_end, self.task_frame[0].orientation(H))
if phi_end == None:
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if (phi_end > pos_traj.phi_end) or (phi_end > ori_traj.phi_end):
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if traj_type == 'regular':
jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity)
elif traj_type == 'polynomial':
jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity)
else:
assert False, "\n Unknown Trajectory Type \n"
if self.ik_settings.respect_limits_in_trajectories:
jt.vel_max = self.ik_settings.max_js
jt.acc_max = self.ik_settings.max_ja
jt.pos_max = self.config_settings.qh
jt.pos_min = self.config_settings.ql
phi = phi_start
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
if relative:
p0 = self.task_point[0].position(H) - pos_traj.current_position
self.set_target([self.task_point[0].position(H)], [self.task_frame[0].orientation(H)])
else:
p0 = np.zeros(3)
self.set_target([pos_traj.current_position], [ori_traj.current_orientation])
self.goto_target()
jt.add_position(0.0, pos = np.copy(self.q))
if self.ik_settings.generate_log:
self.run_log.clear()
self.run_log.step.append(self.report_status())
stay = True
while stay:
phi = phi + self.ik_settings.time_step
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
p = p0 + pos_traj.current_position
self.set_target([p], [ori_traj.current_orientation])
err_reduced = self.moveto_target()
if err_reduced or (not self.ik_settings.respect_error_reduction):
jt.add_position(phi = phi - phi_start, pos = np.copy(self.q))
if traj_type == 'polynomial':
jt.interpolate()
self.set_config(keep_q)
if self.ik_settings.generate_log:
self.run_log.time_elapsed = time.time() - self.run_log.time_start
return jt
def possess(self, point):
x = point.cartesian() - self.center.cartesian()
r2 = sum(x * x)
return gen.equal(r2, self.R * self.R)
def possess(self, point):
x = point.cartesian()
return gen.equal(self.a*x[0] + self.b*x[1] + self.c, 0.0)
def possess(self, point):
x = point.cartesian() - self.center.cartesian()
r2 = sum(x*x)
return gen.equal(r2, self.R*self.R)
def js_project(self, pos_traj, ori_traj = None, phi_start = 0.0, phi_end = None, relative = True, traj_capacity = 2, traj_type = 'regular'):
'''
projects the given taskspace pose trajectory into the jointspace using the numeric inverse kinematics.
The phase starts from phi_start and added by self.ik_settings.time_step in each step.
at any time, if a solution is not found, the target is ignored and no configuration is added to the trajectory
'''
self.damping_factor = self.ik_settings.initial_damping_factor
keep_q = np.copy(self.q)
H = self.transfer_matrices()
if ori_traj == None:
ori_traj = trajlib.Orientation_Path()
ori_traj.add_point(0.0, self.task_frame[0].orientation(H))
ori_traj.add_point(pos_traj.phi_end, self.task_frame[0].orientation(H))
if phi_end == None:
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if (phi_end > pos_traj.phi_end) or (phi_end > ori_traj.phi_end):
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if traj_type == 'regular':
jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity)
elif traj_type == 'polynomial':
jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity)
else:
assert False, "\n Unknown Trajectory Type \n"
if self.ik_settings.respect_limits_in_trajectories:
jt.vel_max = self.ik_settings.max_js
jt.acc_max = self.ik_settings.max_ja
jt.pos_max = self.config_settings.qh
jt.pos_min = self.config_settings.ql
phi = phi_start
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
if relative:
p0 = self.task_point[0].position(H) - pos_traj.current_position
self.set_target([self.task_point[0].position(H)], [self.task_frame[0].orientation(H)])
else:
p0 = np.zeros(3)
self.set_target([pos_traj.current_position], [ori_traj.current_orientation])
self.goto_target()
jt.add_position(0.0, pos = np.copy(self.q))
if self.ik_settings.generate_log:
self.run_log.clear()
self.run_log.step.append(self.report_status())
stay = True
while stay:
phi = phi + self.ik_settings.time_step
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
p = p0 + pos_traj.current_position
self.set_target([p], [ori_traj.current_orientation])
err_reduced = self.moveto_target()
if err_reduced or (not self.ik_settings.respect_error_reduction):
jt.add_position(phi = phi - phi_start, pos = np.copy(self.q))
if traj_type == 'polynomial':
jt.interpolate()
self.set_config(keep_q)
if self.ik_settings.generate_log:
self.run_log.time_elapsed = time.time() - self.run_log.time_start
return jt
def possess(self, point):
x_c = point.cartesian() - self.center.cartesian()
d = np.dot(np.dot((x_c).T, self.M), x_c)
return gen.equal(d, 1.0)
def rotation_matrix(rv, parametrization = 'unit_quaternion', symbolic = False, derivative = False, C = None, S = None, U = None, W = None):
'''
This function returns the rotation matrix corresponding to rotation vector as p(phi)*U where:
phi is the rotation angle and U is the unit vector directing towards the axis of rotation
If type is "Linear Parametrization", then: p(phi) = sin(phi)
if "parametrization = 'unit_quaternion', a unit quaternion must be given. The first element rv[0] is the scalar part and
the next three elements (rv[1], rv[2], rv[3]) represent the vectorial part
if argument derivative is True the derivative of the rotation matrix wrt angle phi (rv[0]) is return.
Arguments "symbolic" and "derivative" are now supported only for angle-axis parametrization. C = cos(phi) , S = sin(phi)
when phi is the rotation angle and [Ux, Uy, Uz] is the axis
'''
if symbolic:
from sympy import Symbol
C = gen.replace_if_none(C, Symbol('c'))
S = gen.replace_if_none(S, Symbol('s'))
U = gen.replace_if_none(U, [Symbol('ux'), Symbol('uy'), Symbol('uz')])
W = gen.replace_if_none(W, Symbol('w'))
u = numpy.zeros((3))
if parametrization == 'angular_spherical':
sin_phi = math.sin(rv[0])
cos_phi = math.cos(rv[0])
u[0] = math.sin(rv[1])*math.cos(rv[2])
u[1] = math.sin(rv[1])*math.sin(rv[2])
u[2] = math.cos(rv[1])
elif parametrization == 'angle_axis':
if symbolic:
sin_phi = S
cos_phi = C
u[0] = U[0]
u[1] = U[1]
u[2] = U[2]
else:
sin_phi = math.sin(rv[0])
cos_phi = math.cos(rv[0])
u[0] = rv[1]
u[1] = rv[2]
u[2] = rv[3]
elif parametrization == 'unit_quaternion':
assert len(rv) == 4
assert gen.equal(numpy.linalg.norm(rv), 1.0)
RM = numpy.eye(3)
a2 = rv[0]**2
b2 = rv[1]**2
c2 = rv[2]**2
d2 = rv[3]**2
ab = 2*rv[0]*rv[1]
ac = 2*rv[0]*rv[2]
ad = 2*rv[0]*rv[3]
bc = 2*rv[1]*rv[2]
bd = 2*rv[1]*rv[3]
cd = 2*rv[2]*rv[3]
RM[0,0] = a2 + b2 - c2 - d2
RM[0,1] = bc - ad
RM[0,2] = bd + ac
RM[1,0] = bc + ad
RM[1,1] = a2 - b2 + c2 - d2
RM[1,2] = cd - ab
RM[2,0] = bd - ac
RM[2,1] = cd + ab
RM[2,2] = a2 - b2 - c2 + d2
return(RM)
else:
p = numpy.linalg.norm(rv)
u = rv/p
if parametrization == 'vectorial_identity':
#p = math.pi + angle_in_range(p)
sin_phi = math.sin(p)
cos_phi = math.cos(p)
elif parametrization == 'vectorial_linear':
assert (p <= 1)
sin_phi = p
cos_phi = math.sqrt(1 - p*p)
elif parametrization == 'vectorial_reduced_Euler_Rodrigues':
assert (p <= 2)
sin_phi_2 = p/2
cos_phi_2 = math.sqrt(1 - p*p/4)
sin_phi = 2*sin_phi_2*cos_phi_2
cos_phi = 2*cos_phi_2*cos_phi_2 - 1
elif parametrization == 'vectorial_Cayley_Gibbs_Rodrigues':
t2 = p*p/4
sin_phi = p/(1 + t2)
cos_phi = (1 - t2)/(1 + t2)
elif parametrization == 'vectorial_Wiener_Milenkovic':
phi_4 = math.atan(p/4)
sin_phi = math.sin(4*phi_4)
cos_phi = math.cos(4*phi_4)
u = vectors_and_matrices.normalize(u)
skew_u = vectors_and_matrices.skew(u)
if derivative:
RM = cos_phi*skew_u + sin_phi*numpy.dot(skew_u,skew_u)
else:
RM = numpy.eye(3) + sin_phi*skew_u + (1 - cos_phi)*numpy.dot(skew_u,skew_u)
return(RM)
def possess(self, point):
x_c = point.cartesian() - self.center.cartesian()
d = np.dot(np.dot((x_c).T, self.M), x_c)
return gen.equal(d, 1.0)
