def basis_error(self, current, target):
'''
return the value of basis_position_error
'''
ers = "Target has not been set"
assert target != None, genpy.err_str(__name__, self.__class__.__name__,
sys._getframe().f_code.co_name,
ers)
if self.settings.representation == 'Cartesian Coordinates':
err = current - target
f = err**self.settings.power
elif self.settings.representation == 'Cylindrical_Coordinates':
p = 3
assert len(self.power) == p
f = numpy.zeros((p))
assert False
elif self.settings.representation == 'Spherical_Coordinates':
p = 3
assert len(self.power) == p
f = numpy.zeros((p))
assert False
else:
ers = self.settings.representation + " is an invalid value for property self.settings.representation"
assert False, genpy.err_str(__name__, self.__class__.__name__,
sys._getframe().f_code.co_name, ers)
return f
def error_jacobian(self):
if self.err_jac == None:
'''
Calculates and updates the error jacobian of the endeffector according to the actual and target poses of the task_points and task_frames and their corresponding metrics
'''
cnt = 0
#Creation of the error jacobian matrix"
self.err_jac = numpy.zeros((self.mp + self.mo, self.config_settings.DOF))
H = self.transfer_matrices()
#For task_points
for tp in self.task_point:
ers = "Target has not been set for some of the taskpoints"
assert tp.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
# Calculating error jacobian for each task_point
tp_ej = tp.error_jacobian(H, self.ajac)
tp_er = tp.error.value(tp.position(H), tp.rd)
#Arranging error jacobian for position
for k in range(0,len(tp_er)):
for j in range(0, self.config_settings.DOF):
self.err_jac[cnt,j] = self.jmc_c[j]*tp_ej[k,j]
cnt = cnt + 1
#For task_frames
for tf in self.task_frame:
ers = "Target has not been set for some of the taskframes"
assert tf.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
# Calculating error jacobian for each reference_orientation
tf_ej = tf.error_jacobian(self.ajac)
tf_er = tf.error.value(tf.orientation(H), tf.rd)
#Arranging error jacobian for orientation
for k in range(0,len(tf_er)):
for j in range(0, self.config_settings.DOF):
self.err_jac[cnt,j] = self.jmc_c[j]*tf_ej[k,j]
cnt = cnt + 1
return self.err_jac
def create(self):
print "Workspace Creation Started. Creation Method: ",self.ws_settings.creation_method
if self.ws_settings.creation_method == 'JSG':
self.create_JSG()
elif self.ws_settings.creation_method == 'RND':
self.create_random()
elif self.ws_settings.creation_method == 'PCL':
if not self.ws_settings.predefined_config_list == None:
i = 0
p = len(self.ws_settings.predefined_config_list)
for q in self.ws_settings.predefined_config_list:
print 'Creating Workspace .... ' + str(i) + ' out of: ' + str (p)
qf = self.free_config(q)
assert self.set_config(qf)
self.config_list.append(qf)
ef_pose = self.pose_to_tuple('actual', self.ws_settings.representation_of_orientation)
self.pose_list.append(ef_pose)
self.ws_settings.number_of_configs = len(self.config_list)
pose_set = set(self.pose_list)
config_tuple_list = [ tuple(c) for c in self.config_list ]
config_set = set(config_tuple_list)
self.pose_tree = kdtree.KDTree.construct_from_data(list(pose_set))
self.config_tree = kdtree.KDTree.construct_from_data(list(config_set))
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'create', 'predefined_config_list has not been set. Workspace can NOT be created !')
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'create', self.ws_settings.creation_method+ ' is not a valid value for creation_method')
def basis_error(self, current, target) :
'''
return the value of basis_position_error
'''
ers = "Target has not been set"
assert target != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
if self.settings.representation == 'Cartesian Coordinates':
err = current - target
f = err**self.settings.power
elif self.settings.representation == 'Cylindrical_Coordinates':
p = 3
assert len(self.power) == p
f = numpy.zeros((p))
assert False
elif self.settings.representation == 'Spherical_Coordinates':
p = 3
assert len(self.power) == p
f = numpy.zeros((p))
assert False
else:
ers = self.settings.representation + " is an invalid value for property self.settings.representation"
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
return f
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 update_virtual_jacobian_multipliers(self):
'''
joint_limit_multipliers are coefficients of forward and inverse mapping to/from limited to/from unlimited jointspace.
i.e: jmc_a is the derivative of q to q_star
(Refer also to the comments of method: initialize in this class)
This method calculates and updates the values of one of the joint limit multipliers: "jmc_c" which is called "joint_limit_jacobian_multipliers"
Each column of the error jacobian is multiplied by the corresponding element of vector "jmc_c": Jstar = Je * diag(jmc_c)
Since the coefficient "jmc_c" for a joint, depends on the current value of that joint, it should be updated everytime "q" changes
The other multipliers: "jmc_a", "jmc_b", "jmc_f" and "jmc_g" do not depend on the joints, therefore do not need to be updated
'''
func_name = "update_virtual_jacobian_multipliers"
for i in range(0,self.config_settings.DOF):
if self.config_settings.joint_handling[i] == 'NM':
self.jmc_c[i] = 1.0
elif self.config_settings.joint_handling[i] == 'LM':
self.jmc_c[i] = 1.0 / self.jmc_a[i]
elif self.config_settings.joint_handling[i] == 'TM':
self.jmc_c[i] = - math.sin(self.qvr[i])/self.jmc_a[i]
elif self.config_settings.joint_handling[i] == 'TGM':
t = self.jmc_f[i] * math.cos(self.qvr[i]) + self.jmc_g[i]
self.jmc_c[i] = - 2.0 * math.sin(self.qvr[i])/(self.jmc_a[i]*(1 + t**2))
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'update_virtual_jacobian_multipliers', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
def update(self, current, target) :
'''
Calculates the error vector between the actual and desired orientations of the corresponding taskframe
'''
be = self.basis_error(current, target)
assert self.settings.weight.shape[1] == len(be)
self.current_value = numpy.dot(self.settings.weight, be) + self.settings.offset
if self.settings.precision_base == 'Axis Angle':
self.is_in_target = True
for k in range(0,3):
if self.settings.required_identical_coordinate[k]:
cos_teta = numpy.dot(target.frame_axis(k), current.frame_axis(k))
if (cos_teta < 1.00000) and (cos_teta > -1.00000):
deviation = abs((180.0/math.pi)*math.acos(cos_teta))
self.is_in_target = self.is_in_target and (deviation < self.settings.precision)
# print "deviation = ", k, deviation, self.settings.precision, self.is_in_target
self.is_in_target = self.is_in_target and (cos_teta > -1.00000)
# print self.is_in_target
elif self.settings.precision_base == 'Error Function':
self.is_in_target = (numpy.linalg.norm(self.current_value) < self.settings.precision)
else:
assert False, genpy.err_str(__name__, self.__class__.__name__,'update', self.settings.precision_base + ' is not a valid value for precision_base')
def set_config(self, qd, set_virtual = True):
len_qd = len(qd)
assert len_qd == self.config_settings.DOF, genpy.err_str(__name__, self.__class__.__name__,'set_config','Length of given config vector is '+str(len_qd)+' which does not match the settings DOF('+str(self.config_settings.DOF)+')')
permission = not self.config_settings.joint_limits_respected
if not permission:
permission = self.joints_in_range(qd)
if permission:
j = 0
for jj in range(0,self.config_settings.njoint):
if self.config_settings.free[jj]:
if self.config_settings.limited[j] and (not self.config_settings.prismatic[jj]):
self.q[jj] = trig.angle_standard_range(qd[j])
else:
self.q[jj] = qd[j]
j = j + 1
self.mapto_virtual()
self.update_virtual_jacobian_multipliers()
return True
else:
print "Warning from configuration.set_config(): Given joints are not in their feasible range"
for i in range(self.config_settings.DOF):
if not self.joint_in_range(i, qd[i]):
print
print "Joint Number: ", i
print "Lower Limit :", self.ql[i]
print "Desired Value:", qd[i]
print "Upper Limit :", self.qh[i]
return False
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 mapto_virtual(self):
qf = self.free_config(self.q)
self.qvr = np.zeros((self.config_settings.DOF))
for i in range(self.config_settings.DOF):
if (not self.config_settings.limited) or (self.config_settings.joint_handling[i] == 'NM'):
self.qvr[i] = qf[i]
elif self.config_settings.joint_handling[i] == 'LM':
assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')'
self.qvr[i] = self.jmc_a[i] * qf[i] + self.jmc_b[i]
elif self.config_settings.joint_handling[i] == 'TM':
'''
ql = self.config_settings.ql[i]
qh = self.config_settings.qh[i]
print "i: ", i, "ql: ", ql," q: ", qf[i],"qh: ", qh, " g: ", self.jmc_g[i], " f: ", self.jmc_f[i], " c: ", (qf[i] - self.jmc_g[i]) / self.jmc_f[i]
'''
assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')'
self.qvr[i] = trig.arccos((qf[i] - self.jmc_g[i]) / self.jmc_f[i])
elif self.config_settings.joint_handling[i] == 'TGM':
assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')'
self.qvr[i] = trig.arccos((math.tan(0.5*qf[i]) - self.jmc_g[i]) / self.jmc_f[i])
else:
print 'Wrong Joint Handling (Free Joint ' + str(i) + '): ' + self.config_settings.joint_handling[i]
assert False, genpy.err_str(__name__, self.__class__.__name__, 'q_to_q', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
def set_acceleration(self, ori_dd, representation=None):
self.clear_acceleration()
if representation == 'vector':
self.pdd = oridd
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__getitem__',
representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation):
self.clear_acceleration()
if representation == 'cartesian':
self.xdd = value
elif representation == 'polar':
self.pdd = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_acceleration', representation + ' is not a valid value for representation')
def interpolate_smart(self, points):
n = len(points)
m = 0
for p in points:
m = m + p.count_constraints()
self.degree = m - 1
A = numpy.zeros((m, m))
i = 0
u = numpy.array([])
for p in points:
if p.x != None:
A[i, 0] = 1
for j in range(1, m):
A[i, j] = A[i, j - 1] * p.t
i = i + 1
u = numpy.append(u, p.x)
if p.v != None:
A[i, 0] = 0.0
A[i, 1] = 1.0
for j in range(2, m):
A[i, j] = j * (p.t**(j - 1))
i = i + 1
u = numpy.append(u, p.v)
if p.a != None:
A[i, 0:2] = 0.0
A[i, 2] = 2.0
for j in range(3, m):
A[i, j] = j * (j - 1) * (p.t**(j - 2))
i = i + 1
u = numpy.append(u, p.a)
try:
self.coeff = numpy.dot(numpy.linalg.inv(A), u)
except:
print genpy.err_str(__name__, self.__class__.__name__,
sys._getframe().f_code.co_name,
'The matrix of coefficients is singular')
print "Matrix of coefficients:"
print A
print
print "Determinent = ", numpy.linalg.det(A)
print
assert False
def goto_target(self):
'''
'''
if self.ik_settings.generate_log:
self.run_log.clear()
if self.ik_settings.include_current_config:
self.initial_config = self.free_config(self.q)
if self.in_target():
return True
if self.ik_settings.run_mode == 'binary_run':
self.run_binary(True)
elif self.ik_settings.run_mode == 'normal_run':
if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI"]:
self.run()
elif self.ik_settings.algorithm == "DLS(ADF)":
self.run_dls_vdf()
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is not a valid value for algorithm")
self.start_node = 0
p = len(self.initial_config_list)
while (not self.in_target()) and (self.start_node < p):
assert self.set_config(self.initial_config_list[self.start_node])
self.initial_config = self.free_config(self.q) # Just for keeping the information of the initial status (Initial status will be shown later in the results log file)
# self.forward_update()
if self.ik_settings.run_mode == 'binary_run':
self.run_binary(True)
elif self.ik_settings.run_mode == 'normal_run':
if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI"]:
self.run()
elif self.ik_settings.algorithm == "DLS(ADF)":
self.run_dls_vdf()
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + ' is not a valid value for algorithm')
self.start_node += 1
return self.in_target()
def goto_target(self):
'''
'''
if self.ik_settings.generate_log:
self.run_log.clear()
if self.ik_settings.include_current_config:
self.initial_config = self.free_config(self.q)
if self.in_target():
return True
if self.ik_settings.run_mode == 'binary_run':
self.run_binary(True)
elif self.ik_settings.run_mode == 'normal_run':
if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI", "DLS(MADF)"]:
self.run()
elif self.ik_settings.algorithm == "DLS(ADF)":
self.run_dls_vdf()
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is not a valid value for algorithm")
self.start_node = 0
p = len(self.initial_config_list)
while (not self.in_target()) and (self.start_node < p):
assert self.set_config(self.initial_config_list[self.start_node])
self.initial_config = self.free_config(self.q) # Just for keeping the information of the initial status (Initial status will be shown later in the results log file)
# self.forward_update()
if self.ik_settings.run_mode == 'binary_run':
self.run_binary(True)
elif self.ik_settings.run_mode == 'normal_run':
if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI", "DLS(MADF)"]:
self.run()
elif self.ik_settings.algorithm == "DLS(ADF)":
self.run_dls_vdf()
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + ' is not a valid value for algorithm')
self.start_node += 1
return self.in_target()
def interpolate_smart(self, points):
n = len(points)
m = 0
for p in points:
m = m + p.count_constraints()
self.degree = m - 1
A = numpy.zeros((m,m))
i = 0
u = numpy.array([])
for p in points:
if p.x != None:
A[i, 0] = 1
for j in range(1, m):
A[i, j] = A[i, j-1]*p.t
i = i + 1
u = numpy.append(u, p.x)
if p.v != None:
A[i, 0] = 0.0
A[i, 1] = 1.0
for j in range(2, m):
A[i, j] = j*(p.t**(j-1))
i = i + 1
u = numpy.append(u, p.v)
if p.a != None:
A[i, 0:2] = 0.0
A[i, 2] = 2.0
for j in range(3, m):
A[i, j] = j*(j-1)*(p.t**(j-2))
i = i + 1
u = numpy.append(u, p.a)
try:
self.coeff = numpy.dot(numpy.linalg.inv(A), u)
except:
print genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, 'The matrix of coefficients is singular')
print "Matrix of coefficients:"
print A
print
print "Determinent = ", numpy.linalg.det(A)
print
assert False
def set_velocity(self, value, representation):
self.clear_velocity()
if representation == 'cartesian':
self.pd = value
elif representation == 'spherical':
self.sd = value
elif representation == 'cylindrical':
self.cd = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_velocity', representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation):
self.clear_acceleration()
if representation == 'cartesian':
self.xdd = value
elif representation == 'polar':
self.pdd = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, 'set_acceleration',
representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation = None):
self.clear_acceleration()
if representation == None:
representation = self.representation
if representation == 'vector':
self.pdd = value
elif representation == 'matrix':
self.Rdd = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_acceleration', representation + ' is not a valid value for representation')
def optimum_stepsize(self, direction):
if self.ik_settings.algorithm in ['JPI','JI']:
k = 1.0
elif self.ik_settings.algorithm in ['JT', 'DLS(CDF)', 'DLS(ADF)', 'DLS(MADF)']:
J_delta_q = np.dot(self.error_jacobian(), direction)
k = - np.sum(J_delta_q*J_delta_q)/np.sum(J_delta_q*self.pose_error())
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is an unknown value for algorithm")
# assert k >= 0.0
return k
def optimum_stepsize(self, direction):
if self.ik_settings.algorithm in ['JPI','JI']:
k = 1.0
elif self.ik_settings.algorithm in ['JT', 'DLS(CDF)', 'DLS(ADF)', 'DLS(MADF)']:
J_delta_q = np.dot(self.error_jacobian(), direction)
k = - np.sum(J_delta_q*J_delta_q)/np.sum(J_delta_q*self.pose_error())
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is an unknown value for algorithm")
# assert k >= 0.0
return k
def set_acceleration(self, value, representation=None):
self.clear_acceleration()
if representation == None:
representation = self.representation
if representation == 'vector':
self.pdd = value
elif representation == 'matrix':
self.Rdd = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, 'set_acceleration',
representation + ' is not a valid value for representation')
def set_velocity(self, value, representation):
self.clear_velocity()
if representation == 'cartesian':
self.pd = value
elif representation == 'spherical':
self.sd = value
elif representation == 'cylindrical':
self.cd = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, 'set_velocity',
representation + ' is not a valid value for representation')
def __setitem__(self, representation, value):
self.clear()
if representation == 'cartesian':
self.x = value
elif representation == 'polar':
'''
p[0] = r
p[1] = theta
'''
self.p = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def pose_error(self):
if self.current_pose_error == None:
'''
Calculates and updates the current values of position and orientation errors between the actual and target poses of task_points and task_frames
Also updates the values of "pose_error" and "error_norm"
'''
#Creation of the vector of errors
cnt = 0
p = self.pose() # This should change
self.num_task_constraints()
self.current_pose_error = numpy.zeros((self.mp + self.mo))
#For task_points
self.all_task_points_in_target = True
H = self.transfer_matrices()
for tp in self.task_point:
ers = "Target has not been set for some of the taskpoints"
assert tp.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
# Calculating position error for each task_point
self.all_task_points_in_target = self.all_task_points_in_target and tp.error.in_target(tp.position(H), tp.rd)
#Arranging the vector of errors for position
tp_err = tp.error.value(tp.ra, tp.rd)
for k in range(0,len(tp_err)):
self.current_pose_error[cnt] = tp_err[k]
cnt = cnt + 1
#For task_frames
self.all_task_frames_in_target = True
for tf in self.task_frame:
ers = "Target has not been set for some of the taskframes"
assert tf.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
# Calculating orientation error for each task_frame
self.all_task_frames_in_target = self.all_task_frames_in_target and tf.error.in_target(tf.orientation(H), tf.rd)
#Arranging the vector of errors for orientation"
tf_err = tf.error.value(tf.ra, tf.rd)
for k in range(0,len(tf_err)):
self.current_pose_error[cnt] = tf_err[k]
cnt = cnt + 1
self.is_in_target = self.all_task_points_in_target and self.all_task_frames_in_target
return self.current_pose_error
def parameter_value(self, parameter):
if parameter == 'AS':
return self.application_scenario
elif parameter == 'AL':
return self.ik_settings.algorithm
elif parameter == 'MAN':
return self.geo_settings.name
elif parameter == 'KC':
return str(self.NKC())
elif parameter == 'MNI':
return str(self.ik_settings.number_of_steps)
elif parameter == 'PP':
return str(1000*self.end_settings.precision_for_position)
elif parameter == 'PO':
return str(self.end_settings.precision_for_rotation)
elif parameter == 'PM':
if self.num_position_ref == 0:
return 'NA'
else:
return self.position_constraint
elif parameter == 'OM':
if self.num_orientation_ref == 0:
return 'NA'
else:
return self.orientation_constraint
elif parameter == 'JM':
if 'TM' in self.config_settings.joint_handling:
return 'TM'
elif 'LM' in self.config_settings.joint_handling:
return 'LM'
else:
return 'NM'
# return genpy.most_common(self.config_settings.joint_handling)
elif parameter == 'DF':
if self.ik_settings.algorithm[0:3] == 'DLS':
return str(self.ik_settings.initial_damping_factor)
else:
return 'NA'
elif parameter == 'DFG':
if self.ik_settings.algorithm == 'DLS(ADF)':
return str(self.ik_settings.df_gain)
elif self.ik_settings.algorithm == 'DLS(MADF)':
return str(self.ik_settings.manipulability_threshold)
else:
return 'NA'
elif parameter == 'WCTP':
return self.workspace_settings_tp.gen_key()
elif parameter == 'WCIC':
return self.workspace_settings_ic.gen_key()
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'parameter_value', parameter + ' is an Unknown Setting Parameter')
def nearest_configs(self, ik):
if self.ws_settings.search_criteria == 'NTP': # (key: NTP) Initial Configuration: Nearest to Target Pose
nearest_config_list = self.configs_nearest_to_pose(ik)
elif self.ws_settings.search_criteria == 'NCC': # (key: NCC) Initial Configuration: Nearest to Current Configuration
nearest_config_list = self.configs_nearest_to(ik.free_config(ik.q))
elif self.ws_settings.search_criteria == 'CLO':
nearest_config_list = []
for i in range(self.ws_settings.number_of_search_items):
nearest_config_list.append(self.config_list[i])
else :
assert False, genpy.err_str(__name__, self.__class__.__name__, 'nearest_configs', self.ws_settings.search_criteria + ' is not a valid value for search_criteria')
return nearest_config_list
def set_config_virtual(self, qvrd):
self.qvr = qvrd
qd = self.mapfrom_virtual(qvrd)
permission = not self.config_settings.joint_limits_respected
if not permission:
permission = self.joints_in_range(qd)
if permission:
self.q = self.free_config_inv(qd)
self.update_virtual_jacobian_multipliers()
return True
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'set_config_virtual', 'The joint values computed from qvr are out of range while joint limits are respected.')
return False
def __setitem__(self, representation, value):
self.clear()
if representation == 'matrix':
self.R = value
elif representation == 'quaternion':
self.Q = value
elif representation == 'angle_axis':
self.phi = value[0]
self.u = value[1:4]
elif representation == 'vector':
self.p = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value):
self.clear()
if representation == 'cartesian':
self.x = value
elif representation == 'polar':
'''
p[0] = r
p[1] = theta
'''
self.p = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__setitem__',
representation + ' is not a valid value for representation')
def __getitem__(self, representation):
if representation == 'cartesian':
return self.cartesian()
elif representation == 'cartesian_velocity':
return self.cartesian_velocity()
elif representation == 'cartesian_acceleration':
return self.cartesian_acceleration()
elif representation == 'polar':
return self.polar()
elif representation == 'polar_velocity':
return self.polar_velocity()
elif representation == 'polar_acceleration':
return self.polar_acceleration()
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value):
self.clear()
if representation == 'matrix':
self.R = value
elif representation == 'quaternion':
self.Q = value
elif representation == 'angle_axis':
self.phi = value[0]
self.u = value[1:4]
elif representation == 'vector':
self.p = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__setitem__',
representation + ' is not a valid value for representation')
def __getitem__(self, representation):
if representation == 'cartesian':
return self.cartesian()
elif representation == 'cartesian_velocity':
return self.cartesian_velocity()
elif representation == 'cartesian_acceleration':
return self.cartesian_acceleration()
elif representation == 'polar':
return self.polar()
elif representation == 'polar_velocity':
return self.polar_velocity()
elif representation == 'polar_acceleration':
return self.polar_acceleration()
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__getitem__',
representation + ' is not a valid value for representation')
def objective_function_gradient(self, k = 1.0):
dof = self.config_settings.DOF
qh = self.qh
ql = self.ql
u = np.zeros(dof)
qs = trig.angles_standard_range(self.qvr)
for i in range(0, self.config_settings.DOF):
if self.config_settings.joint_handling[i] == 'TM':
u[i] = - k*qs[i]/(dof*((2*math.pi)**2))
elif self.config_settings.joint_handling[i] == 'NM':
if self.config_settings.limited[i]:
mean = 0.5*(self.qh[i] + self.ql[i])
u[i] = - k*(qs[i] - mean)/(dof*((self.qh[i] - self.ql[i])**2))
# print "qs[i] = ", qs[i], " mean = ", mean, " qh-ql= ", self.qh[i] - self.ql[i], " u = ", u[i]
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'objective_function_gradient', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
return u
def __getitem__(self, representation):
if representation == 'matrix':
return self.matrix()
elif representation == 'quaternion':
return self.quaternion()
elif representation == 'angle_axis':
return self.angle_axis()
elif representation == 'angle':
return self.angle()
elif representation == 'axis':
return self.axis()
elif representation == 'vector':
return self.vector()
elif representation == 'trace':
return np.trace(self.matrix())
elif representation == 'diag':
return np.diag(self.matrix())
elif representation == 'angular_spherical':
return self.spherical()
elif representation == 'matrix_velocity':
return self.matrix_velocity()
elif representation == 'quaternion_velocity':
return self.quaternion_velocity()
elif representation == 'angle_axis_velocity':
u = self.axis_velocity()
return np.array([self.angle_velocity(), u[0], u[1], u[2]])
elif representation == 'angle_velocity':
return self.angle_velocity()
elif representation == 'axis_velocity':
return self.axis_velocity()
elif representation == 'vector_velocity':
return self.vector_velocity()
elif representation == 'vector_acceleration':
return self.vector_acceleration()
elif representation == 'trace_velocity':
return np.trace(self.matrix_velocity())
elif representation == 'diag_velocity':
return np.diag(self.matrix_velocity())
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__getitem__',
representation + ' is not a valid value for representation')
def value(self, figure):
if figure == 'Time':
return self.current_time
elif figure == 'Duration':
return self.time_elapsed
elif figure == 'Phase':
return self.phase
elif figure == 'Manipulability':
return self.manipulability
elif figure == 'Damping Factor':
return self.damping_factor
elif figure == 'In Range':
return self.joints_in_range
elif figure == 'Target Reached':
return self.is_in_target
elif figure == 'Error Norm':
return self.error_norm
else:
ers = figure + ' is not a valid value for argument figure'
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
def objective_function_value(self, k = 1.0):
dof = self.config_settings.DOF
qs = trig.angles_standard_range(self.qvr)
qh = self.qh
ql = self.ql
f = 0.0
for i in range(0, self.config_settings.DOF):
df = 0.0
if self.config_settings.joint_handling[i] == 'TM':
f += - k*qs[i]/(dof*((2*math.pi)**2))
elif self.config_settings.joint_handling[i] == 'NM':
if self.config_settings.limited[i]:
mean = 0.5*(self.qh[i] + self.ql[i])
df = k*((qs[i] - mean)**2)/(dof*((self.qh[i] - self.ql[i])**2))
# print "qs[i] = ", qs[i], " mean = ", mean, " qh-ql= ", self.qh[i] - self.ql[i], " df = ", df
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'objective_function_value', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
f += df
return f
def mapfrom_virtual(self, qs):
qq = np.zeros((self.config_settings.DOF))
for i in range(self.config_settings.DOF):
if self.config_settings.limited[i]:
if self.config_settings.joint_handling[i] == 'NM':
qq[i] = trig.angle_standard_range( qs[i] )
elif self.config_settings.joint_handling[i] == 'LM':
qs[i] = trig.angle_standard_range( qs[i] )
qq[i] = (qs[i] - self.jmc_b[i]) / self.jmc_a[i]
elif self.config_settings.joint_handling[i] == 'TM':
qq[i] = self.jmc_f[i] * math.cos(qs[i]) + self.jmc_g[i]
elif self.config_settings.joint_handling[i] == 'TGM':
qq[i] = 2*math.atan(self.jmc_f[i] * math.cos(qs[i]) + self.jmc_g[i])
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'mapfrom_virtual', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
else:
qq[i] = qs[i]
# return self.free_config_inv(qq)
return qq
def __setitem__(self, representation, value):
self.clear()
if representation == 'cartesian':
self.p = value
elif representation == 'spherical':
'''
s[0] = ro
s[1] = theta
s[2] = phi
'''
self.s = value
elif representation == 'cylindrical':
'''
c[0] = r
c[1] = theta
c[2] = z
'''
self.c = value
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def value(self, figure):
if figure == 'Time':
return self.current_time
elif figure == 'Duration':
return self.time_elapsed
elif figure == 'Phase':
return self.phase
elif figure == 'Manipulability':
return self.manipulability
elif figure == 'Damping Factor':
return self.damping_factor
elif figure == 'In Range':
return self.joints_in_range
elif figure == 'Target Reached':
return self.is_in_target
elif figure == 'Error Norm':
return self.error_norm
else:
ers = figure + ' is not a valid value for argument figure'
assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
def __getitem__(self, representation):
if representation == 'matrix':
return self.matrix()
elif representation == 'quaternion':
return self.quaternion()
elif representation == 'angle_axis':
return self.angle_axis()
elif representation == 'angle':
return self.angle()
elif representation == 'axis':
return self.axis()
elif representation == 'vector':
return self.vector()
elif representation == 'trace':
return np.trace(self.matrix())
elif representation == 'diag':
return np.diag(self.matrix())
elif representation == 'angular_spherical':
return self.spherical()
elif representation == 'matrix_velocity':
return self.matrix_velocity()
elif representation == 'quaternion_velocity':
return self.quaternion_velocity()
elif representation == 'angle_axis_velocity':
u = self.axis_velocity()
return np.array([self.angle_velocity(), u[0], u[1], u[2]])
elif representation == 'angle_velocity':
return self.angle_velocity()
elif representation == 'axis_velocity':
return self.axis_velocity()
elif representation == 'vector_velocity':
return self.vector_velocity()
elif representation == 'vector_acceleration':
return self.vector_acceleration()
elif representation == 'trace_velocity':
return np.trace(self.matrix_velocity())
elif representation == 'diag_velocity':
return np.diag(self.matrix_velocity())
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value):
self.clear()
if representation == 'cartesian':
self.p = value
elif representation == 'spherical':
'''
s[0] = ro
s[1] = theta
s[2] = phi
'''
self.s = value
elif representation == 'cylindrical':
'''
c[0] = r
c[1] = theta
c[2] = z
'''
self.c = value
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, '__setitem__',
representation + ' is not a valid value for representation')
def update(self, current, target):
'''
Calculates the error vector between the actual and desired orientations of the corresponding taskframe
'''
be = self.basis_error(current, target)
assert self.settings.weight.shape[1] == len(be)
self.current_value = numpy.dot(self.settings.weight,
be) + self.settings.offset
if self.settings.precision_base == 'Axis Angle':
self.is_in_target = True
for k in range(0, 3):
if self.settings.required_identical_coordinate[k]:
cos_teta = numpy.dot(target.frame_axis(k),
current.frame_axis(k))
if (cos_teta < 1.00000) and (cos_teta > -1.00000):
deviation = abs(
(180.0 / math.pi) * math.acos(cos_teta))
self.is_in_target = self.is_in_target and (
deviation < self.settings.precision)
# print "deviation = ", k, deviation, self.settings.precision, self.is_in_target
self.is_in_target = self.is_in_target and (cos_teta >
-1.00000)
# print self.is_in_target
elif self.settings.precision_base == 'Error Function':
self.is_in_target = (numpy.linalg.norm(self.current_value) <
self.settings.precision)
else:
assert False, genpy.err_str(
__name__, self.__class__.__name__, 'update',
self.settings.precision_base +
' is not a valid value for precision_base')
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 basis_error(self, current, target) :
# def basis_orientation_error(self, current, target):
rpn = self.settings.representation
if rpn in ['vector', 'ReRoAn + ReOrVe']:
current.set_parametrization( self.settings.parametrization )
target.set_parametrization( self.settings.parametrization )
'''
'''
if self.settings.metric_type == 'relative':
Oe = current/target
e = Oe[rpn]
elif self.settings.metric_type == 'differential':
e = current[rpn] - target[rpn]
elif self.settings.metric_type == 'special':
if self.settings.representation == 'AxInPr':
p = 3
assert len(self.settings.power) == p
f = numpy.zeros((p))
for k in range (0,p):
if self.settings.power[k] == 0:
err = 0
f[k] = 1
else:
err = numpy.dot(target.frame_axis(k), current.frame_axis(k))
if self.settings.power[k] == 1 :
f[k] = err
else:
f[k] = err**self.settings.power[k]
elif self.settings.representation == 'DiNoQu':
p = 3
assert len(self.settings.power) == p
f = numpy.zeros((p))
qn = current['quaternion']
qnd = target['quaternion']
for k in range (0,3):
z = - qnd[0]*qn[k+1] + qn[0]*qnd[k+1]
f[k] = z**self.settings.power[k]
elif self.settings.representation == 'AxInPr + DiNoQu':
p = 6
assert len(self.settings.power) == p
f = numpy.zeros((p))
for k in range (0,3):
if self.settings.power[k] == 0:
err = 0
f[k] = 1
else:
err = numpy.dot(target.frame_axis(k), current.frame_axis(k))
if self.settings.power[k] == 1 :
f[k] = err
else:
f[k] = err**self.settings.power[k]
qn = current['quaternion']
qnd = target['quaternion']
for k in range (3,6):
z = - qnd[0]*qn[k-2] + qn[0]*qnd[k-2]
f[k] = z**self.settings.power[k]
elif self.settings.representation == 'ReRoAn + ReOrVe':
p = 4
assert len(self.settings.power) == p
e = numpy.zeros((p))
Oe = current/target
e[0] = Oe['angle']
e[1:4] = Oe['vector']
f = e**self.settings.power
else:
assert False
return f
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, "basis_error", self.settings.metric_type + " is an invalid metric type")
if self.settings.representation == 'matrix':
e = e.flatten()
f = e**self.settings.power
return f
def __init__(self, representation = "Cartesian Coordinates", metric_type = "differential"):
assert (representation in representations_of_position) or (representation in representations_of_orientation) or (representation in special_metric_representations), genpy.err_str(__name__,self.__class__.__name__,'__init__', representation+' is an invalid value for representation')
self.metric_type = metric_type
self.representation = representation
self.parametrization = 'identity'
'''
Property "power" determines the power of elements of basis error in the final vector of errors:
Please refer to the comments of property: "W" in this file.
0: Not considered for the corresponding coordinate
1: rd - ra
2: (rd - ra)^2
3: (rd - ra)^3
.
.
.
where:
"ra" is the actual position
"rd" is the desired position
and r can be replaced by: X, Y or Z
'''
self.power = numpy.array([1, 1, 1])
'''
property "W" is a 3 X 3 Weighting matrix. This matrix will be multiplied by the basis error and basis jacobian.
Matrix W is by default the identity matrix defining 3 constraints as:
Xd - X = 0
Yd - Y = 0
Zd - Z = 0
Example 1:
"power" array = [1, 1, 1] and "W" is a (3 X 3) matrix
X Y Z
1st Row of "W": 1 0 0 (Xd - Xa) = 0
2nd Row of "W": 0 1 0 (Yd - Ya) = 0
3rd Row of "W": 0 0 1 (Zd - Za) = 0
Example 2:
"power" array = [2, 2, 1] and "W" is a (2 X 3) matrix
X Y Z
1st Row of "W": 1 1 0 (Xd - Xa)^2 + (Yd - Ya)^2 = 0
2nd Row of "W": 0 0 1 (Zd - Za) = 0
Example 3: (When only two coordinations "X" and "Z" are important to be identical)
In this example, it is important to also set the property "required_identical_coordinate" to: [True, False, True]
Please refer to the of comments for property "required_identical_coordinate".
"power" array = [1, 0, 1] and "W" is a (2 X 3) matrix
X Y Z
1st Row of "W": 1 0 0 (Xd - Xa) = 0
2nd Row of "W": 0 0 1 (Zd - Za) = 0
the same for the orientation ...
'''
self.weight = numpy.eye(3)
self.offset = numpy.zeros((3))
# for orientation:
if self.representation in ['diag', 'AxInPr']:
self.offset = numpy.array([-1.0, -1.0, -1.0])
elif self.representation in ['trace', 'angle']:
self.weight = numpy.array([[1]])
self.power = numpy.array([1])
if self.representation == 'trace':
self.offset = numpy.array([-3.0])
else:
self.offset = numpy.array([0.0])
else:
self.offset = numpy.zeros(3)
'''
"self.required_identical_coordinate" is an array of booleans and indicates which coordinates (x, y ,z) should be considered
as a criteria in determining the confirmity of the actual and desired positions and orientations.
For example if "required_identical_coordinate" = [True, False, True] the actual and desired positions are considered as identical only when
their "x" and "z" coordinates are equal.
(the "y" coordinate will be neglected)
Please refer to "example 3" of the comments provided for property: "W" in this file.
'''
self.required_identical_coordinate = [True, True, True]
'''
set "precision" or termination criteria by default as: 2 cm. It means actual and desired positions are considered "identical"
if for each position coordinate (x, y, z), the absolute value of the difference of current and desired, does not exceed: 2 cm.
'''
if representation in representations_of_position:
self.precision = 0.01
self.precision_base = "Coordinate Difference"
else:
self.precision = 1.0
self.precision_base = "Axis Angle"
def set_acceleration(self, ori_dd, representation = None):
self.clear_acceleration()
if representation == 'vector':
self.pdd = oridd
else:
assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def initialize(self):
'''
Everything regarding joint parameters needed to be done before running kinematic calculations
This function must be called whenever you change joint limits or change type of a joint.
'''
func_name = "initialize()"
# Counting free joints in order to detremine the degree of freedom: "DOF"
self.config_settings.DOF = 0
# Important: Here you do not consider prismatic joints. Do it in the future
for i in range(0,self.config_settings.njoint):
# First bring revolute joint limits in the range (- pi , pi)
if (not self.config_settings.prismatic[i]):
self.config_settings.ql[i] = trig.angle_standard_range( self.config_settings.ql[i] )
self.config_settings.qh[i] = trig.angle_standard_range( self.config_settings.qh[i] )
if self.config_settings.free[i]:
self.config_settings.DOF += 1
self.ql = self.free_config(self.config_settings.ql)
self.qh = self.free_config(self.config_settings.qh)
# check all the joints are already in their desired range
'''
for i in range(self.DOF):
if not self.config_settings.joint_handling[i] == 'No Mapping':
assert self.joints_in_range() #check only for joint i
'''
'''
The following code defines the joint limit multipliers
jmc_a and jmc_b are auxiliary coefficients which are used for conversion from "q" to "qvr" space
(Please refer to the joint limits section of the documentation associated with this code)
To make sure that all joints are in their desired range identified as: (self.ql, self.qh), the real jointspace is mapped into an unlimited jointspace.
"q " represent the real joint values which are in a limited jointpace
"qvr" represent the mapped joint values in unlimited jointspace
The following code creates and defines "Joint Limit Multipliers". These are coefficients for this jointspace mapping - (jmc: Jointspace Mapping Coefficients)
in linear mapping:
q = a * qvr + b
qs = (q - b) / a
in trigonometric mapping:
q = f * cos(qvr) + g
qvr = arccos [ (q - g) / f ]
a, b, f and g are calculated in such a way that the real joint values which are in their feasible range are mapped into an equivalent qvr in an unlimited space (-pi, +pi)
"jmc_c" is multiplied by the columns of the error jacobian matrix
'''
self.jmc_a = np.zeros((self.config_settings.DOF))
self.jmc_b = np.zeros((self.config_settings.DOF))
self.jmc_c = np.zeros((self.config_settings.DOF))
self.jmc_f = np.zeros((self.config_settings.DOF))
self.jmc_g = np.zeros((self.config_settings.DOF))
self.jmc_f = 0.5*(self.qh - self.ql)
self.jmc_g = 0.5*(self.qh + self.ql)
for i in range(0, self.config_settings.DOF):
if self.config_settings.joint_handling[i] == 'NM':
self.jmc_a[i] = 1.00000
self.jmc_b[i] = 0.00000
elif self.config_settings.joint_handling[i] == 'LM':
self.jmc_a[i] = 2*math.pi/(self.qh[i] - self.ql[i])
self.jmc_b[i] = math.pi * (self.qh[i] + self.ql[i])/(self.ql[i] - self.qh[i])
elif self.config_settings.joint_handling[i] == 'TM':
self.jmc_a[i] = 2.0/(self.qh[i] - self.ql[i])
self.jmc_b[i] = (self.qh[i] + self.ql[i])/(self.ql[i] - self.qh[i])
elif self.config_settings.joint_handling[i] == 'TGM':
th = math.tan(0.5*self.qh[i])
tl = math.tan(0.5*self.ql[i])
self.jmc_a[i] = 2/(th - tl)
self.jmc_b[i] = (tl + th)/(tl - th)
self.jmc_f[i] = 0.5*(th - tl)
self.jmc_g[i] = 0.5*(th + tl)
else:
assert False, genpy.err_str(__name__, self.__class__.__name__, 'initialize', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
# map q to qvr for the first time:
self.mapto_virtual()
# assign the value of jmc_c the jacobian multiplier:
self.update_virtual_jacobian_multipliers()
