import numpy as np
import sympy as sp
import re
import os

######################
#                    #
#    17   16   21    #
#  18     15     22  #
#  19     14     23  #
#  20     01     24  #
#      02    08      #
#     03      09     #
#     04      10     #
#     05      11     #
#     06      12     #
#     07      13     #
#                    #
######################
#
# origin: in the waist, middle point between the two pitch hip rotations
# inertial frame: located at the origin (waist), but aligned with the ground (info from IMU)
#
# Di   : position vector from the anchor point of the previous body to the current body i 
#        (previous body is not always body i-1), expressed in the relative
#        frame of the previous body
# DGi  : position vector from the anchor point of body i to its COM (center of mass) G_i,
#        expressed in the relative frame of the current body i
# Omi  : rotational vector from the previous body to the current body i 
#        (previous body is not always body i-1), expressed in the relative
#        frame of the previous body
# Rdi  : rotational matrix between body i and its predecessor
# si   : sine of the relative angle before body i
# ci   : cosine of the relative angle before body i
#
# xi   : absolute position vector (from origin, expressed in the inertial frame) 
#        of the anchor point of body i
# xgi  : absolute position vector of the COM G_i of body i
# xpi  : derivative of xi
# xgpi : derivative of xgi
# omi  : absolute rotational vector of body i
# Ri   : absolute rotational matrix
# Rti  : transpose matrix of Ri
# xji  : jacobian of 'xi'
# xgji : jacobian of 'xgi'
# Rji  : jacobian of 'Ri'

# return true if it is a float
def isInt(value):
	try:
		int(value)
		return True
	except:
		return False

# return true if it has a shape 'R%a_%b%c' (indexes %a, %b, %c also returned)
def isRot(value):
	try:
		a = int(value.split('_')[0].split('R')[1])
		b = int(value.split('_')[1][0])
		c = int(value.split('_')[1][1])
		return True, a, b, c
	except:
		return False, -1, -1, -1

# return true if it has a shape 'x%a_%b' (indexes %a, %b also returned)
def isVec(value):
	try:
		a = int(value.split('_')[0].split('x')[1])
		b = int(value.split('_')[1])
		return True, a, b
	except:
		return False, -1, -1

# count the number of 'elem' in the file
def count_elem(in_file, elem):

	count = 0;

	with open(in_file, 'r') as f:

		# loop on all the lines
		for line in f:
			cut_line = line.split(elem)
			if len(cut_line) == 2:
				count += 1

	return count

# print the declaration of an element
def print_declaration_elem(in_file, out_write, elem, nb_max_line):

	if count_elem(in_file, '{}'.format(elem)) >= 1:

		count = 0

		with open(in_file,'r') as f:

			# loop on all the lines
			for line in f:
				cut_line_1 = line.split(elem)
				cut_line_2 = line.split(' = ')

				if len(cut_line_1) == 2 and len(cut_line_2) == 2:
					if len(cut_line_2[0].split('[')) == 1:
						if count == 0:
							out_write.write('	double {}'.format(cut_line_2[0].strip()))
						else:
							out_write.write(', {}'.format(cut_line_2[0].strip()))
						count += 1

						if count >= nb_max_line:
							out_write.write(';\n')
							count = 0

		if count != 0:
			out_write.write(';\n')

# print all declarations
def print_all_declaration(in_file, out_write, nb_max_char):

	count = 0

	with open(in_file,'r') as f:

		# loop on all the lines
		for line in f:
			cut_line = line.split(' = ')

			if len(cut_line) == 2:
				if len(cut_line[0].split('[')) == 1:				
					if count == 0:
						out_write.write('	double {}'.format(cut_line[0].strip()))
					else:
						out_write.write(', {}'.format(cut_line[0].strip()))
					count += len(cut_line[0].strip()) + 2

					if count >= nb_max_char:
						out_write.write(';\n')
						count = 0

	if count != 0:
		out_write.write(';\n')

# get tilde matrix
def get_tilde(v):
	return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])

# get rotation matrix
def get_rotation_matrix(axis, direct, cosine, sine):

	if direct:
		if axis == 1:
			return np.array([[1.0, 0.0, 0.0], [0.0, cosine, sine], [0.0, -sine, cosine]])
		elif axis == 2:
			return np.array([[cosine, 0.0, -sine], [0.0, 1.0, 0.0], [sine, 0.0, cosine]])
		elif axis == 3:
			return np.array([[cosine, sine, 0.0], [-sine, cosine, 0.0], [0.0, 0.0, 1.0]])
		else:
			return np.array([])
	else:
		if axis == 1:
			return np.array([[1.0, 0.0, 0.0], [0.0, cosine, -sine], [0.0, sine, cosine]])
		elif axis == 2:
			return np.array([[cosine, 0.0, sine], [0.0, 1.0, 0.0], [-sine, 0.0, cosine]])
		elif axis == 3:
			return np.array([[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]])
		else:
			return np.array([])

# get vector axis
def get_vector_axis(axis, direct, elem):

	if direct:
		if axis == 1:
			return np.array([[elem], [0.0], [0.0]])
		elif axis == 2:
			return np.array([[0.0], [elem], [0.0]])
		elif axis == 3:
			return np.array([[0.0], [0.0], [elem]])
		else:
			return np.array([])
	else:
		if axis == 1:
			return np.array([[-elem], [0.0], [0.0]])
		elif axis == 2:
			return np.array([[0.0], [-elem], [0.0]])
		elif axis == 3:
			return np.array([[0.0], [0.0], [-elem]])
		else:
			return np.array([])

# compute the derivative of an element (for jacobian)
def der_elem(elem_str, Rj, xj, xgj, der_var):

	# element to derive (string)
	elem_str = elem_str.replace('- ','-').strip()

	# derivative axis
	der_q = int(der_var.replace('q',''))

	# detect positive/negative
	elem_split = elem_str.split('-')
	cur_len = len(elem_split)

	if cur_len == 1: # positive
		neg_flag = 0
		pos_str = elem_split[0]
	elif cur_len == 2: # negative
		neg_flag = 1
		pos_str = elem_split[1]
	else:
		print('Error: {} instead of 1 or 2 in negative detection !'.format(cur_len))
		exit()

	# compute derivative
	result = 0

	# cosine
	if pos_str == 'c{}'.format(der_q):
		result += -sp.Symbol('s{}'.format(der_q))

	# sine
	elif pos_str == 's{}'.format(der_q):
		result += sp.Symbol('c{}'.format(der_q))

	# other
	else:
		[rot_flag, a, b, c] = isRot(pos_str)
		[vec_flag, d, e] = isVec(pos_str)

		# rotation matrix
		if rot_flag:
			result += Rj[a-1][der_q-1][(b-1)*3+(c-1)]

		# vector
		elif vec_flag:
			result += xj[d-1][der_q-1][e-1]

	# apply negative
	if neg_flag:
		result = -result

	return result

# compute the derivative of an expression (for jacobian)
def symbolic_jacob_der(Rj, xj, xgj, symb_var, der_var):

	# list of all terms
	term_list = str(symb_var).replace('- ','-').replace('-','+-').split('+')

	if term_list[0] == '':
		term_list.pop(0)

	result = 0

	# loop on all terms
	for cur_term in term_list:

		# detect products
		cur_term_split = cur_term.split('*')
		cur_len = len(cur_term_split)

		# no product
		if cur_len == 1:
			result += der_elem(cur_term_split[0], Rj, xj, xgj, der_var)

		# one product
		elif cur_len == 2:
			result += der_elem(cur_term_split[0], Rj, xj, xgj, der_var)*sp.Symbol(cur_term_split[1].strip())
			result += der_elem(cur_term_split[1], Rj, xj, xgj, der_var)*sp.Symbol(cur_term_split[0].strip())
		
		# other
		else:
			print('Error: {} * counted , only implemented for 0 or 1 !'.format(cur_len-1))
			exit()

	return result

# write the beginning of the file
def write_file_beginning(out_file, joint_id_names):
	out_file.write('/*! \n')
	out_file.write(' * \\author Nicolas Van der Noot\n')
	out_file.write(' * \\file forward_kinematics.cc\n')
	out_file.write(' * \\brief forward kinematics computation for the COMAN model\n')
	out_file.write(' */\n\n')

	out_file.write('// joints enumeration\n')
	out_file.write('enum {')
	count = 0

	for i in range(1, len(joint_id_names)):

		count += 1

		if i == 1:
			out_file.write('{}'.format(get_string_enum(joint_id_names[i])))
		elif count >= 6:
			count = 0
			out_file.write(',\n	  {}'.format(get_string_enum(joint_id_names[i])))
		else:
			out_file.write(', {}'.format(get_string_enum(joint_id_names[i])))

	out_file.write('};\n\n')

	out_file.write('/*! \\brief main kinematics computation\n')
	out_file.write(' *\n')
	out_file.write(' * \\param[in,out] in_out inputs and outputs class\n')
	out_file.write(' *\n')
	out_file.write(' * computation of:\n')
	out_file.write(' *     COM (center of mass) position and velocity\n')
	out_file.write(' *     feet position, velocity and orientation\n')
	out_file.write(' *     waist and torso orientaion angles and derivatives\n')
	out_file.write(' *\n')
	out_file.write(' *   ////////////////////////\n')
	out_file.write(' *   //                    //\n')
	out_file.write(' *   //    17   16   21    //\n')
	out_file.write(' *   //  18     15     22  //\n')
	out_file.write(' *   //  19     14     23  //\n')
	out_file.write(' *   //  20     01     24  //\n')
	out_file.write(' *   //      02    08      //\n')
	out_file.write(' *   //     03      09     //\n')
	out_file.write(' *   //     04      10     //\n')
	out_file.write(' *   //     05      11     //\n')
	out_file.write(' *   //     06      12     //\n')
	out_file.write(' *   //     07      13     //\n')
	out_file.write(' *   //                    //\n')
	out_file.write(' *   ////////////////////////\n')
	out_file.write(' *\n')
	out_file.write(' * origin: in the waist, middle point between the two pitch hip rotations\n')
	out_file.write(' * inertial frame: located at the origin (waist), but aligned with the ground (info from IMU)\n')
	out_file.write(' *\n')
	out_file.write(' * Di   : position vector from the anchor point of the previous body to the current body i \n')
	out_file.write(' *        (previous body is not always body i-1), expressed in the relative\n')
	out_file.write(' *        frame of the previous body\n')
	out_file.write(' * DGi  : position vector from the anchor point of body i to its COM (center of mass) G_i,\n')
	out_file.write(' *        expressed in the relative frame of the current body i\n')
	out_file.write(' * Omi  : rotational vector from the previous body to the current body i \n')
	out_file.write(' *        (previous body is not always body i-1), expressed in the relative\n')
	out_file.write(' *        frame of the previous body\n')
	out_file.write(' * Rdi  : rotational matrix between body i and its predecessor\n')
	out_file.write(' * si   : sine of the relative angle before body i\n')
	out_file.write(' * ci   : cosine of the relative angle before body i\n')
	out_file.write(' *\n')
	out_file.write(' * xi   : absolute position vector (from origin, expressed in the inertial frame)\n')
	out_file.write(' *        of the anchor point of body i\n')
	out_file.write(' * xgi  : absolute position vector of the COM G_i of body i\n')
	out_file.write(' * xpi  : derivative of xi\n')
	out_file.write(' * xgpi : derivative of xgi\n')
	out_file.write(' * omi  : absolute rotational vector of body i\n')
	out_file.write(' * Ri   : absolute rotational matrix\n')
	out_file.write(' * Rti  : transpose matrix of Ri\n')
	out_file.write(' * xji  : jacobian of \'xi\'\n')
	out_file.write(' * xgji : jacobian of \'xgi\'\n')
	out_file.write(' * Rji  : jacobian of \'Ri\'\n')
	out_file.write(' */\n')

	out_file.write('void ForwardKinematics::main_kinematics(KinematicsInOut &in_out)\n{\n')

# compute the center of mass position and velocity
def com_compute(out_file, nb_bodies, joint_id_names, M, xg, xgp, xgj):

	out_file.write('	m_tot = ')

	for i in range(0, nb_bodies):
		out_file.write('{}'.format(M[i]))
		if i == nb_bodies-1:
			out_file.write(';\n\n')
		else:
			out_file.write(' + ')

	out_file.write('	// global com absolute position\n')
	for i in range(0, 3):
		out_file.write('	in_out.r_COM[{}] = '.format(i))
		flag_first = 0
		for j in range(0, nb_bodies):
			if flag_first:
				out_file.write(' + {}*{}'.format(M[j], xg[j][i]))
			else:
				flag_first = 1
				out_file.write('({}*xg{}_{}'.format(M[j], j+1, i+1))
			if j == nb_bodies-1:
				if flag_first:
					out_file.write(')/m_tot;\n')
				else:
					out_file.write('0.0;\n')

	out_file.write('\n')

	out_file.write('	// global com absolute velocity\n')
	for i in range(0, 3):
		out_file.write('	in_out.rp_COM[{}] = '.format(i))
		flag_first = 0
		for j in range(0, nb_bodies):
			if flag_first:
				out_file.write(' + {}*xgp{}_{}'.format(M[j], j+1, i+1))
			else:
				flag_first = 1
				out_file.write('({}*xgp{}_{}'.format(M[j], j+1, i+1))
			if j == nb_bodies-1:
				if flag_first:
					out_file.write(')/m_tot;\n')
				else:
					out_file.write('0.0;\n')
	out_file.write('\n')

	out_file.write('	// global com jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	for i in range(1, nb_bodies):
		for j in range(0, 3):
			out_file.write('		in_out.r_COM_der[{}][{}] = '.format(get_string_enum(joint_id_names[i]), j))
			flag_first = 0
			for k in range(0, nb_bodies):
				if xgj[k][i][j] != 0:
					if flag_first:
						out_file.write(' + {}*{}'.format(M[k], str(xgj[k][i][j])))
					else:
						flag_first = 1
						out_file.write('({}*{}'.format(M[k], str(xgj[k][i][j])))
				if k == nb_bodies-1:
					if flag_first:
						out_file.write(')/m_tot;\n')
					else:
						out_file.write('0.0;\n')
		if i != nb_bodies-1:
			out_file.write('\n')
		else:
			out_file.write('	}\n\n')

# from an orientation matrix, compute the roll, pitch, yaw angles (and derivative)
def yaw_pitch_roll_angles(out_file, angle_name, R_matrix, epsilon):

	if epsilon > 0: # epsilon = 1 -> pitch angle in [-pi/2 ; pi/2]
		out_file.write('	in_out.{}[0] = atan2({}, {});\n'.format(angle_name, R_matrix[5], R_matrix[8]))
		out_file.write('	in_out.{}[1] = atan2(-{}, sqrt({}*{} + {}*{}));\n'.format(angle_name, R_matrix[2], R_matrix[0], R_matrix[0], R_matrix[1], R_matrix[1]))
		out_file.write('	in_out.{}[2] = atan2({}, {});\n'.format(angle_name, R_matrix[1], R_matrix[0]))
	else: # epsilon = -1 -> pitch angle in [pi/2 ; 3*pi/2]
		out_file.write('	in_out.{}[0] = atan2(-{}, -{});\n'.format(angle_name, R_matrix[5], R_matrix[8]))
		out_file.write('	in_out.{}[1] = atan2(-{}, -sqrt({}*{} + {}*{}));\n'.format(angle_name, R_matrix[2], R_matrix[0], R_matrix[0], R_matrix[1], R_matrix[1]))
		out_file.write('	in_out.{}[2] = atan2(-{}, -{});\n'.format(angle_name, R_matrix[1], R_matrix[0]))	

# compute the time derivatives of 'yaw_pitch_roll_angles'
def theta_dot_compute(out_file, omega_in, omega_out, body_part):

	out_file.write('	in_out.{}[0] = inv_c_y_{} * (c_z_{}*{} + s_z_{}*{});\n'.format(omega_out, body_part, body_part, omega_in[0], body_part, omega_in[1]))
	out_file.write('	in_out.{}[1] = c_z_{}*{} - s_z_{}*{};\n'.format(omega_out, body_part, omega_in[1], body_part, omega_in[0]))
	out_file.write('	in_out.{}[2] = inv_c_y_{} * s_y_{} * (s_z_{}*{} + c_z_{}*{}) + {};\n'.format(omega_out, body_part, body_part, body_part, omega_in[1], body_part, omega_in[0], omega_in[2]))

# angles (position and derivative) of the waist and the torso
def torso_waist_angles(out_file, R, om, waist_id, torso_id):

	out_file.write('	// waist orientation matrix as angles [rad]\n')
	yaw_pitch_roll_angles(out_file, 'theta_waist', R[waist_id], 1)
	out_file.write('\n')

	out_file.write('	// torso orientation matrix as angles [rad]\n')
	yaw_pitch_roll_angles(out_file, 'theta_torso', R[torso_id], 1)
	out_file.write('\n')

	out_file.write('	c_y_waist = cos(in_out.theta_waist[1]);\n')
	out_file.write('	c_y_torso = cos(in_out.theta_torso[1]);\n')
	out_file.write('	c_z_waist = cos(in_out.theta_waist[2]);\n')
	out_file.write('	c_z_torso = cos(in_out.theta_torso[2]);\n\n')
	out_file.write('	s_y_waist = sin(in_out.theta_waist[1]);\n')
	out_file.write('	s_y_torso = sin(in_out.theta_torso[1]);\n')
	out_file.write('	s_z_waist = sin(in_out.theta_waist[2]);\n')
	out_file.write('	s_z_torso = sin(in_out.theta_torso[2]);\n\n')
	out_file.write('	if ((!c_y_waist) || (!c_y_torso))\n	{\n')
	out_file.write('		return;\n	}\n\n')
	out_file.write('	inv_c_y_waist = 1.0 / c_y_waist;\n')
	out_file.write('	inv_c_y_torso = 1.0 / c_y_torso;\n\n')

	out_file.write('	// waist orientation angle derivatives [rad/s]\n')
	theta_dot_compute(out_file, om[waist_id], 'omega_waist', 'waist')
	out_file.write('\n')

	out_file.write('	// torso orientation angle derivatives [rad/s]\n')
	theta_dot_compute(out_file, om[torso_id], 'omega_torso', 'torso')


# compute the feet position, velocity and orientation
def feet_compute(out_file, joint_id_names, R, x, xp, om, Rj, xj, xgj, r_foot_id, l_foot_id, x_min, x_max, y_min, y_max):

	# symbolic variables declarations
	nb_contacts = 4

	x_r_foot = x[r_foot_id]
	x_l_foot = x[l_foot_id]

	xp_r_foot = xp[r_foot_id]
	xp_l_foot = xp[l_foot_id]

	om_r_foot = om[r_foot_id]
	om_l_foot = om[l_foot_id]

	R_r_foot = R[r_foot_id]
	R_l_foot = R[l_foot_id]

	Dpt_r_foot = sp.zeros(3, 1)
	Dpt_l_foot = sp.zeros(3, 1)

	Dpt_r_foot[2] = sp.Symbol('DPT_3_16')
	Dpt_l_foot[2] = sp.Symbol('DPT_3_29')

	Dpt_r_foot_cont = nb_contacts * [None]
	Dpt_l_foot_cont = nb_contacts * [None]

	for i in range(0, nb_contacts):
		Dpt_r_foot_cont[i] = sp.zeros(3, 1)
		Dpt_l_foot_cont[i] = sp.zeros(3, 1)

	Dpt_r_foot_cont[0][0] = x_min
	Dpt_r_foot_cont[1][0] = x_min
	Dpt_r_foot_cont[2][0] = x_max
	Dpt_r_foot_cont[3][0] = x_max

	Dpt_r_foot_cont[0][1] = y_min
	Dpt_r_foot_cont[1][1] = y_max
	Dpt_r_foot_cont[2][1] = y_min
	Dpt_r_foot_cont[3][1] = y_max

	for i in range(0, nb_contacts):
		Dpt_r_foot_cont[i][2] = sp.Symbol('DPT_3_16')

	for i in range(0, nb_contacts):
		for j in range(0, 3):
			Dpt_l_foot_cont[i][j] = Dpt_r_foot_cont[i][j]

	x_r_cont = nb_contacts * [None]
	x_l_cont = nb_contacts * [None]

	# computation
	om_tilde_r_foot = get_tilde(om_r_foot)
	om_tilde_l_foot = get_tilde(om_l_foot)

	x_r = x_r_foot  + R_r_foot.T * Dpt_r_foot
	x_l = x_l_foot  + R_l_foot.T * Dpt_l_foot

	xp_r = xp_r_foot + om_tilde_r_foot * (R_r_foot.T * Dpt_r_foot)
	xp_l = xp_l_foot + om_tilde_l_foot * (R_l_foot.T * Dpt_l_foot)

	for i in range(0, nb_contacts):
		x_r_cont[i] = x_r_foot  + R_r_foot.T * Dpt_r_foot_cont[i]
		x_l_cont[i] = x_l_foot  + R_l_foot.T * Dpt_l_foot_cont[i]

	# writing outputs
	out_file.write('	// right foot absolute position\n')
	for i in range(0,3):
		out_file.write('	in_out.r_Rfoot[{}] = {};\n'.format(i, x_r[i]))
	out_file.write('\n')

	out_file.write('	// right foot absolute velocity\n')
	for i in range(0,3):
		out_file.write('	in_out.rp_Rfoot[{}] = {};\n'.format(i, xp_r[i]))
	out_file.write('\n')

	out_file.write('	// right foot jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0, 3):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_r[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.r_Rfoot_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// left foot absolute position\n')
	for i in range(0,3):
		out_file.write('	in_out.r_Lfoot[{}] = {};\n'.format(i, x_l[i]))
	out_file.write('\n')

	out_file.write('	// left foot absolute velocity\n')
	for i in range(0,3):
		out_file.write('	in_out.rp_Lfoot[{}] = {};\n'.format(i, xp_l[i]))
	out_file.write('\n')

	out_file.write('	// left foot jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0, 3):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_l[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.r_Lfoot_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// right foot contact points absolute position\n')
	for i in range(0, nb_contacts):
		for j in range(0, 3):
			out_file.write('	in_out.r_Rfoot_cont[{}][{}] = {};\n'.format(i, j, x_r_cont[i][j]))
		out_file.write('\n')

	out_file.write('	// right foot contact points jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range(0, nb_contacts):
		for j in range (1, nb_bodies):
			flag_print = 0
			for k in range(0, 3):
				cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_r_cont[i][k], 'q{}'.format(j+1))
				if cur_jac != 0:
					if not flag_first:
						flag_first = 1
						flag_print = 1
					elif not flag_print:
						flag_print = 1
						out_file.write('\n')
					out_file.write('		in_out.r_Rfoot_cont_der[{}][{}][{}] = {};\n'.format(i, get_string_enum(joint_id_names[j]), k, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// left foot contact points absolute position\n')
	for i in range(0, nb_contacts):
		for j in range(0, 3):
			out_file.write('	in_out.r_Lfoot_cont[{}][{}] = {};\n'.format(i, j, x_l_cont[i][j]))
		out_file.write('\n')

	out_file.write('	// left foot contact points jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range(0, nb_contacts):
		for j in range (1, nb_bodies):
			flag_print = 0
			for k in range(0, 3):
				cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_l_cont[i][k], 'q{}'.format(j+1))
				if cur_jac != 0:
					if not flag_first:
						flag_first = 1
						flag_print = 1
					elif not flag_print:
						flag_print = 1
						out_file.write('\n')
					out_file.write('		in_out.r_Lfoot_cont_der[{}][{}][{}] = {};\n'.format(i, get_string_enum(joint_id_names[j]), k, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// feet absolute orientation\n')
	for i in range(0, 9):
		out_file.write('	in_out.Rfoot_or[{}] = {};\n'.format(i, R_r_foot[i]))
	out_file.write('\n')

	for i in range(0, 9):
		out_file.write('	in_out.Lfoot_or[{}] = {};\n'.format(i, R_l_foot[i]))
	out_file.write('\n')

	out_file.write('	// right foot absolute orientation jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0,9):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, R_r_foot[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.Rfoot_or_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// left foot absolute orientation jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0,9):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, R_l_foot[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.Lfoot_or_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// right foot orientation matrix as angles [rad]\n')
	yaw_pitch_roll_angles(out_file, 'theta_Rfoot', R[r_foot_id], 1)
	out_file.write('\n')

	out_file.write('	// left foot orientation matrix as angles [rad]\n')
	yaw_pitch_roll_angles(out_file, 'theta_Lfoot', R[l_foot_id], 1)
	out_file.write('\n')

	out_file.write('	c_y_Rfoot = cos(in_out.theta_Rfoot[1]);\n')
	out_file.write('	c_y_Lfoot = cos(in_out.theta_Lfoot[1]);\n')
	out_file.write('	c_z_Rfoot = cos(in_out.theta_Rfoot[2]);\n')
	out_file.write('	c_z_Lfoot = cos(in_out.theta_Lfoot[2]);\n\n')
	out_file.write('	s_y_Rfoot = sin(in_out.theta_Rfoot[1]);\n')
	out_file.write('	s_y_Lfoot = sin(in_out.theta_Lfoot[1]);\n')
	out_file.write('	s_z_Rfoot = sin(in_out.theta_Rfoot[2]);\n')
	out_file.write('	s_z_Lfoot = sin(in_out.theta_Lfoot[2]);\n\n')
	out_file.write('	if ((!c_y_Rfoot) || (!c_y_Lfoot))\n	{\n')
	out_file.write('		return;\n	}\n\n')
	out_file.write('	inv_c_y_Rfoot = 1.0 / c_y_Rfoot;\n')
	out_file.write('	inv_c_y_Lfoot = 1.0 / c_y_Lfoot;\n\n')

	out_file.write('	// right foot orientation angle derivatives [rad/s]\n')
	theta_dot_compute(out_file, om[r_foot_id], 'omega_Rfoot', 'Rfoot')
	out_file.write('\n')

	out_file.write('	// left foot orientation angle derivatives [rad/s]\n')
	theta_dot_compute(out_file, om[l_foot_id], 'omega_Lfoot', 'Lfoot')
	out_file.write('\n')

# compute the wrists position, velocity and orientation
def wrists_compute(out_file, joint_id_names, R, x, xp, om, Rj, xj, xgj, r_elb_id, l_elb_id, r_wrist_x, r_wrist_y, r_wrist_z):

	# symbolic variables declarations
	x_r_elb = x[r_elb_id]
	x_l_elb = x[l_elb_id]

	xp_r_elb = xp[r_elb_id]
	xp_l_elb = xp[l_elb_id]

	om_r_elb = om[r_elb_id]
	om_l_elb = om[l_elb_id]

	R_r_elb = R[r_elb_id]
	R_l_elb = R[l_elb_id]

	Dpt_r_wrist = sp.zeros(3, 1)
	Dpt_l_wrist = sp.zeros(3, 1)

	Dpt_r_wrist[0] = r_wrist_x
	Dpt_r_wrist[1] = r_wrist_y
	Dpt_r_wrist[2] = r_wrist_z

	Dpt_l_wrist[0] =  r_wrist_x
	Dpt_l_wrist[1] = -r_wrist_y
	Dpt_l_wrist[2] =  r_wrist_z

	# computation
	om_tilde_r_elb = get_tilde(om_r_elb)
	om_tilde_l_elb = get_tilde(om_l_elb)

	x_r = x_r_elb  + R_r_elb.T * Dpt_r_wrist
	x_l = x_l_elb  + R_l_elb.T * Dpt_l_wrist

	xp_r = xp_r_elb + om_tilde_r_elb * (R_r_elb.T * Dpt_r_wrist)
	xp_l = xp_l_elb + om_tilde_l_elb * (R_l_elb.T * Dpt_l_wrist)

	# writing outputs
	out_file.write('	// right wrist absolute position\n')
	for i in range(0,3):
		out_file.write('	in_out.r_Rwrist[{}] = {};\n'.format(i, x_r[i]))
	out_file.write('\n')

	out_file.write('	// right wrist absolute velocity\n')
	for i in range(0,3):
		out_file.write('	in_out.rp_Rwrist[{}] = {};\n'.format(i, xp_r[i]))
	out_file.write('\n')

	out_file.write('	// right wrist jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0, 3):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_r[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.r_Rwrist_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// left wrist absolute position\n')
	for i in range(0,3):
		out_file.write('	in_out.r_Lwrist[{}] = {};\n'.format(i, x_l[i]))
	out_file.write('\n')

	out_file.write('	// left wrist absolute velocity\n')
	for i in range(0,3):
		out_file.write('	in_out.rp_Lwrist[{}] = {};\n'.format(i, xp_l[i]))
	out_file.write('\n')

	out_file.write('	// left wrist jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0, 3):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, x_l[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.r_Lwrist_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// wrists absolute orientation\n')
	for i in range(0, 9):
		out_file.write('	in_out.Rwrist_or[{}] = {};\n'.format(i, R_r_elb[i]))
	out_file.write('\n')

	for i in range(0, 9):
		out_file.write('	in_out.Lwrist_or[{}] = {};\n'.format(i, R_l_elb[i]))
	out_file.write('\n')

	out_file.write('	// right wrist absolute orientation jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0,9):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, R_r_elb[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.Rwrist_or_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

	out_file.write('	// left wrist absolute orientation jacobian\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range (1, nb_bodies):
		flag_print = 0
		for j in range(0,9):
			cur_jac = symbolic_jacob_der(Rj, xj, xgj, R_l_elb[j], 'q{}'.format(i+1))
			if cur_jac != 0:
				if not flag_first:
					flag_first = 1
					flag_print = 1
				elif not flag_print:
					flag_print = 1
					out_file.write('\n')
				out_file.write('		in_out.Lwrist_or_der[{}][{}] = {};\n'.format(get_string_enum(joint_id_names[i]), j, cur_jac))
	out_file.write('	}\n\n')

# get a string for the enumeration of joints
def get_string_enum(cur_string):

	cur_split = cur_string.split('_')

	if len(cur_split) >= 2:
		new_string = cur_split[0]
		for i in range(1, len(cur_split)-1):
			new_string = '{}{}'.format(new_string, cur_split[i])
	else:
		new_string = cur_string
	
	cur_split = filter(None, re.split("([A-Z][^A-Z]*)", new_string))

	new_string = cur_split[0].upper()

	for i in range(1, len(cur_split)):
		new_string = '{}_{}'.format(new_string, cur_split[i].upper())

	return new_string

# write the end of the file
def write_file_end(out_file):
	out_file.write('}\n')

# print matrix components declaration
def write_matrix_declaration(out_file, prefix):
	out_file.write('	double ')
	for i in range(0,3):
		for j in range(0,3):
			out_file.write('{}{}{}'.format(prefix, i+1, j+1))
			if i == 2 and j == 2:
				out_file.write(';\n')
			else:
				out_file.write(', ')

# print variables declaration
def write_variables_declaration(out_file, prefix, min, max):
	out_file.write('	double ')
	for i in range(min, max+1):
		out_file.write('{}{}'.format(prefix, i))
		if i == max:
			out_file.write(';\n')
		else:
			out_file.write(', ')

# variables initialization
def write_intialization(out_file, nb_bodies, joint_id_names):

	out_file.write('	// -- variables initialization -- //\n')

	out_file.write('\n	// IMU - rotation matrices\n')
	for i in range(0, 3):
		for j in range(0, 3):
			out_file.write('	IMU{}{} = in_out.IMU_Orientation[{}];\n'.format(i+1, j+1, 3*i+j))

	out_file.write('\n	// IMU - angles velocity\n')
	for i in range(0, 3):
		out_file.write('	omega_{} = in_out.IMU_Angular_Rate[{}];\n'.format(i+1, i))

	out_file.write('\n	// joint cosines\n')
	for i in range(1, nb_bodies):
		out_file.write('	c{} = cos(in_out.q_mot[{}]);\n'.format(i+1, joint_id_names[i]))

	out_file.write('\n	// joint sines\n')
	for i in range(1, nb_bodies):
		out_file.write('	s{} = sin(in_out.q_mot[{}]);\n'.format(i+1, joint_id_names[i]))

	out_file.write('\n	// joint relative velocities\n')
	for i in range(1, nb_bodies):
		out_file.write('	Om{} = in_out.qd_mot[{}];\n'.format(i+1, joint_id_names[i]))

# write symbolic vector and replace symbolic variable by its name
def write_symb_vector(out_file, vector, start_name, end_name):

	new_vector = sp.zeros(3, 1)

	flag_print = 0

	for i in range(0,3):

		if vector[i] == 0 or vector[i] == 1:
			new_vector[i] = vector[i]
		else:
			flag_print = 1

			elem_name = '{}{}{}'.format(start_name, i+1, end_name)
			out_file.write('	{} = {};\n'.format(elem_name, vector[i]).replace('1.0*',''))

			new_vector[i] = sp.Symbol(elem_name)

	if flag_print:
		out_file.write('\n')

	return new_vector

# write symbolic matrix and replace symbolic variable by its name
def write_symb_matrix(out_file, matrix, start_name, end_name):

	new_matrix = sp.zeros(3, 3)

	flag_print = 0

	for i in range(0,3):
		for j in range(0,3):

			if matrix[i,j] == 0 or matrix[i,j] == 1:
				new_matrix[i,j] = matrix[i,j]
			else:
				flag_print = 1

				elem_name = '{}{}{}{}'.format(start_name, i+1, j+1, end_name)
				out_file.write('	{} = {};\n'.format(elem_name, matrix[i,j]).replace('1.0*',''))

				new_matrix[i,j] = sp.Symbol(elem_name)

	if flag_print:
		out_file.write('\n')

	return new_matrix

# save the symbolic vector for print
def print_save_symb_vector(vector, start_name, end_name):

	new_vector  = sp.zeros(3, 1)
	save_vector = 3 * [None]

	for i in range(0,3):

		if vector[i] == 0 or vector[i] == 1:
			new_vector[i]  = vector[i]
			save_vector[i] = None
		else:
			elem_name = '{}{}{}'.format(start_name, i+1, end_name)
			save_vector[i] = '		{} = {};\n'.format(elem_name, vector[i]).replace('1.0*','')

			new_vector[i] = sp.Symbol(elem_name)

	return new_vector, save_vector

# save the symbolic matrix for print
def print_save_symb_matrix(matrix, start_name, end_name):

	new_matrix  = sp.zeros(3, 3)
	save_matrix = 9 * [None]

	for i in range(0,3):
		for j in range(0,3):

			if matrix[i,j] == 0 or matrix[i,j] == 1:
				new_matrix[i,j] = matrix[i,j]
				save_matrix[3*i+j] = None
			else:
				elem_name = '{}{}{}{}'.format(start_name, i+1, j+1, end_name)
				save_matrix[3*i+j] = '		{} = {};\n'.format(elem_name, matrix[i,j]).replace('1.0*','')

				new_matrix[i,j] = sp.Symbol(elem_name)

	return new_matrix, save_matrix

# write symbolic jacobian of a rotation matrix
def write_symb_Rj(nb_bodies, Rj, xj, xgj, Rj_print, R_matrix, index):

	# loop on all the joints
	for i in range (1, nb_bodies):

		new_matrix = sp.zeros(3, 3)

		# loop on all the matrix elements
		for j in range(0, 9):

			new_matrix[j] = symbolic_jacob_der(Rj, xj, xgj, R_matrix[j], 'q{}'.format(i+1))

		[Rj[index-1][i], Rj_print[index-1][i]] = print_save_symb_matrix(new_matrix, 'R{}_'.format(index), '_d{}'.format(i+1))

# write symbolic jacobian of an anchor point
def write_symb_xj(nb_bodies, Rj, xj, xgj, xj_print, x_vector, index):

	# loop on all the joints
	for i in range (1, nb_bodies):

		new_vector = sp.zeros(3, 1)

		# loop on all the vector elements
		for j in range(0, 3):
			new_vector[j] = symbolic_jacob_der(Rj, xj, xgj, x_vector[j], 'q{}'.format(i+1))

		[xj[index-1][i], xj_print[index-1][i]] = print_save_symb_vector(new_vector, 'x{}_'.format(index), '_d{}'.format(i+1))

# write symbolic jacobian of a com point
def write_symb_xgj(nb_bodies, Rj, xj, xgj, xgj_print, x_vector, index):

	# loop on all the joints
	for i in range (1, nb_bodies):

		new_vector = sp.zeros(3, 1)

		# loop on all the vector elements
		for j in range(0, 3):
			new_vector[j] = symbolic_jacob_der(Rj, xj, xgj, x_vector[j], 'q{}'.format(i+1))

		[xgj[index-1][i], xgj_print[index-1][i]] = print_save_symb_vector(new_vector, 'xg{}_'.format(index), '_d{}'.format(i+1))

# symbolic computation
def symbolic_computation(out_file, nb_bodies, joint_id_names, rot_axis, parent_body_index, Dpt, Dg, M):

	out_file.write('\n\n	// -- symbolic computation -- //\n')

	# Rj, xj, xgj and xgj (jacobian)
	Rj  = nb_bodies*[None]
	xj  = nb_bodies*[None]
	xgj = nb_bodies*[None]

	Rj_print  = nb_bodies*[None]
	xj_print  = nb_bodies*[None]
	xgj_print = nb_bodies*[None]
	
	for i in range(0, nb_bodies):
		Rj[i]  = nb_bodies*[None]
		xj[i]  = nb_bodies*[None]
		xgj[i] = nb_bodies*[None]

		Rj_print[i]  = nb_bodies*[None]
		xj_print[i]  = nb_bodies*[None]
		xgj_print[i] = nb_bodies*[None]

		for j in range(0, nb_bodies-1):
			Rj[i][j]  = sp.zeros(3, 3)
			xj[i][j]  = sp.zeros(3, 1)
			xgj[i][j] = sp.zeros(3, 1)

			Rj_print[i][j]  = 9 * [None]
			xj_print[i][j]  = 3 * [None]
			xgj_print[i][j] = 3 * [None]


	# rotation matrices
	out_file.write('\n	// rotation matrices\n')

	R  = nb_bodies*[None]
	Rt = nb_bodies*[None]
	Rd = nb_bodies*[None]

	Rd[0] = sp.zeros(3, 3)
	R[0]  = sp.zeros(3, 3)
	for i in range(0, 3):
		for j in range(0, 3):
			R[0][i,j] = sp.Symbol('IMU{}{}'.format(i+1, j+1))
	write_symb_Rj(nb_bodies, Rj, xj, xgj, Rj_print, R[0], 1)
	R[0]  = write_symb_matrix(out_file, R[0], 'R1_', '')
	Rt[0] = R[0].T

	for i in range(1, nb_bodies):
		Rd[i] = get_rotation_matrix(rot_axis[i], 1, sp.Symbol('c{}'.format(i+1)), sp.Symbol('s{}'.format(i+1)))
		R[i]  = Rd[i] * R[parent_body_index[i]]
		write_symb_Rj(nb_bodies, Rj, xj, xgj, Rj_print, R[i], i+1)
		R[i]  = write_symb_matrix(out_file, R[i], 'R{}_'.format(i+1), '')
		Rt[i] = R[i].T

	# jacobian rotation matrices
	out_file.write('\n	// jacobian rotation matrices\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range(0, nb_bodies):
		for j in range(1, nb_bodies):
			flag_print = 0
			for k in range(0, 9):
				if Rj_print[i][j][k] != None:
					if not flag_first:
						flag_first = 1
						flag_print = 1
					elif not flag_print:
						flag_print = 1
						out_file.write('\n')
					out_file.write('{}'.format(Rj_print[i][j][k]))
	out_file.write('	}\n')

	# omega
	out_file.write('\n	// joint absolute velocities\n')

	Om = nb_bodies*[None]
	om = nb_bodies*[None]
	om_tilde = nb_bodies*[None]

	Om[0] = sp.zeros(3, 1)
	om[0] = sp.zeros(3, 1)
	for i in range(0,3):
		om[0][i] = sp.Symbol('omega_{}'.format(i+1))
	om[0] = write_symb_vector(out_file, om[0], 'om1_', '')
	om_tilde[0] = get_tilde(om[0])

	for i in range(1, nb_bodies):
		parent_id = parent_body_index[i]
		Om[i] = get_vector_axis(rot_axis[i], 1, sp.Symbol('Om{}'.format(i+1)))
		om[i] = om[parent_id] + Rt[parent_id] * Om[i]
		om[i] = write_symb_vector(out_file, om[i], 'om{}_'.format(i+1), '')
		om_tilde[i] = get_tilde(om[i])


	# x & xp
	out_file.write('\n	// anchor point absolute positions and velocities\n')

	x  = nb_bodies*[None]
	xp = nb_bodies*[None]

	x[0]  = Rt[0] * Dpt[0]
	xp[0] = om_tilde[0] * (Rt[0] * Dpt[0])
	write_symb_xj(nb_bodies, Rj, xj, xgj, xj_print, x[0], 1)
	x[0]  = write_symb_vector(out_file, x[0], 'x1_', '')
	xp[0] = write_symb_vector(out_file, xp[0], 'xp1_', '')

	for i in range(1, nb_bodies):
		parent_id = parent_body_index[i]
		x[i]  = x[parent_id]  + Rt[parent_id] * Dpt[i]
		xp[i] = xp[parent_id] + om_tilde[parent_id] * (Rt[parent_id] * Dpt[i])
		write_symb_xj(nb_bodies, Rj, xj, xgj, xj_print, x[i], i+1)
		x[i]  = write_symb_vector(out_file, x[i], 'x{}_'.format(i+1), '')
		xp[i] = write_symb_vector(out_file, xp[i], 'xp{}_'.format(i+1), '')


	# jacobian x
	out_file.write('\n	// jacobian anchor point positions\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range(0, nb_bodies):
		for j in range(1, nb_bodies):
			flag_print = 0
			for k in range(0, 3):
				if xj_print[i][j][k] != None:
					if not flag_first:
						flag_first = 1
						flag_print = 1
					elif not flag_print:
						flag_print = 1
						out_file.write('\n')
					out_file.write('{}'.format(xj_print[i][j][k]))
	out_file.write('	}\n')


	# xg & xgp
	out_file.write('\n	// com absolute positions and velocities\n')

	xg  = nb_bodies*[None]
	xgp = nb_bodies*[None]

	for i in range(0, nb_bodies):
		xg[i] = x[i] + Rt[i] * Dg[i]
		xgp[i] = xp[i] + om_tilde[i] * (Rt[i] * Dg[i])
		write_symb_xgj(nb_bodies, Rj, xj, xgj, xgj_print, xg[i], i+1)
		xg[i]  = write_symb_vector(out_file, xg[i], 'xg{}_'.format(i+1), '')
		xgp[i] = write_symb_vector(out_file, xgp[i], 'xgp{}_'.format(i+1), '')


	# jacobian xg
	out_file.write('\n	// jacobian com absolute positions\n')
	out_file.write('	if (flag_jacob)\n	{\n')
	flag_first = 0
	for i in range(0, nb_bodies):
		for j in range(1, nb_bodies):
			flag_print = 0
			for k in range(0, 3):
				if xgj_print[i][j][k] != None:
					if not flag_first:
						flag_first = 1
						flag_print = 1
					elif not flag_print:
						flag_print = 1
						out_file.write('\n')
					out_file.write('{}'.format(xgj_print[i][j][k]))
	out_file.write('	}\n')


	# results
	out_file.write('\n	// -- Collecting results -- //\n\n')

	com_compute(out_file, nb_bodies, joint_id_names, M, xg, xgp, xgj)

	feet_compute(out_file, joint_id_names, R, x, xp, om, Rj, xj, xgj, 6, 12, -0.06, 0.08, -0.045, 0.045)

	wrists_compute(out_file, joint_id_names, R, x, xp, om, Rj, xj, xgj, 19, 23, -0.02, -0.005, -0.225)

	torso_waist_angles(out_file, R, om, 0, 15)


# generate the symbolic output file
def gen_symbolic_out(out_file_name, nb_bodies, rot_axis, parent_body_index, joint_id_names, Dpt, Dg, M):

	# temporary file
	in_temp = './{}_temp.cc'.format(out_file_name)
	file_temp = open(in_temp, 'w')

	# beginning of the file
	write_file_beginning(file_temp, joint_id_names)

	# variables initialization
	write_intialization(file_temp, nb_bodies, joint_id_names)

	# symbolic computation
	symbolic_computation(file_temp, nb_bodies, joint_id_names, rot_axis, parent_body_index, Dpt, Dg, M)	

	# end of the file
	write_file_end(file_temp)

	file_temp.close()

	# output file
	out_file = open('./{}.cc'.format(out_file_name), 'w')

	with open(in_temp, 'r') as f:

		# loop on all the lines
		for line in f:

			# declaration
			if len(line.split('// -- variables initialization -- //')) != 1:

				out_file.write('	// -- variables declaration -- //\n\n')
				print_all_declaration(in_temp, out_file, 100)
				out_file.write('\n\n')

			# copy temporary file
			out_file.write(line)

	out_file.close()

	# remove temporary file
	os.remove(in_temp)


# main script

# rotation axis for each joint before body i (1:x, 2:y, 3:z)
rot_axis = np.array([0,       # waist
			2, 1, 3, 2, 1, 2, # right leg
			2, 1, 3, 2, 1, 2, # left leg
			1, 2, 3,          # trunk
			2, 1, 3, 2,       # right arm
			2, 1, 3, 2        # left arm
			])

# parent index
parent_body_index = np.array([   -1, # waist
			0,   1,   2,  3,  4,  5, # right leg
			0,   7,   8,  9, 10, 11, # left leg
			0,  13,  14,             # trunk
			15, 16,  17,  18,        # right arm
			15, 20,  21,  22         # left arm
			])

nb_bodies = len(parent_body_index)

## anchor point positions
Dpt = nb_bodies*[None]

# waist
Dpt[0] = sp.Matrix([0.0, 0.0, 0.0])

# right leg
Dpt[1] = sp.Matrix([0.0, sp.Symbol('DPT_2_2'), 0.0])
Dpt[2] = sp.Matrix([0.0, sp.Symbol('DPT_2_6'), 0.0])
Dpt[3] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_8')])
Dpt[4] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_10')])
Dpt[5] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_12')])
Dpt[6] = sp.Matrix([0.0, 0.0, 0.0])

# left leg
Dpt[7]  = sp.Matrix([0.0, sp.Symbol('DPT_2_3'), 0.0])
Dpt[8]  = sp.Matrix([0.0, sp.Symbol('DPT_2_18'), 0.0])
Dpt[9]  = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_20')])
Dpt[10] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_22')])
Dpt[11] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_24')])
Dpt[12] = sp.Matrix([0.0, 0.0, 0.0])

# trunk
Dpt[13] = sp.Matrix([sp.Symbol('DPT_1_4'), 0.0, sp.Symbol('DPT_3_4')])
Dpt[14] = sp.Matrix([0.0, 0.0, 0.0])
Dpt[15] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_32')])

# right arm
Dpt[16] = sp.Matrix([sp.Symbol('DPT_1_36'), sp.Symbol('DPT_2_36'), sp.Symbol('DPT_3_36')])
Dpt[17] = sp.Matrix([0.0, sp.Symbol('DPT_2_39'), 0.0])
Dpt[18] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_41')])
Dpt[19] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_43')])

# left arm
Dpt[20] = sp.Matrix([sp.Symbol('DPT_1_37'), sp.Symbol('DPT_2_37'), sp.Symbol('DPT_3_37')])
Dpt[21] = sp.Matrix([0.0, sp.Symbol('DPT_2_46'), 0.0])
Dpt[22] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_48')])
Dpt[23] = sp.Matrix([0.0, 0.0, sp.Symbol('DPT_3_50')])

## COM positions
Dg = nb_bodies*[None]

# waist
Dg[0] = sp.Matrix([sp.Symbol('L_1_6'), sp.Symbol('L_2_6'), sp.Symbol('L_3_6')])

# right leg
Dg[1] = sp.Matrix([sp.Symbol('L_1_7') , sp.Symbol('L_2_7') , sp.Symbol('L_3_7')])
Dg[2] = sp.Matrix([sp.Symbol('L_1_8') , sp.Symbol('L_2_8') , sp.Symbol('L_3_8')])
Dg[3] = sp.Matrix([sp.Symbol('L_1_9') , sp.Symbol('L_2_9') , sp.Symbol('L_3_9')])
Dg[4] = sp.Matrix([sp.Symbol('L_1_10'), sp.Symbol('L_2_10'), sp.Symbol('L_3_10')])
Dg[5] = sp.Matrix([sp.Symbol('L_1_11'), sp.Symbol('L_2_11'), sp.Symbol('L_3_11')])
Dg[6] = sp.Matrix([sp.Symbol('L_1_12'), 0.0                , sp.Symbol('L_3_12')])

# left leg
Dg[7]  = sp.Matrix([sp.Symbol('L_1_13'), sp.Symbol('L_2_13'), sp.Symbol('L_3_13')])
Dg[8]  = sp.Matrix([sp.Symbol('L_1_14'), sp.Symbol('L_2_14'), sp.Symbol('L_3_14')])
Dg[9]  = sp.Matrix([sp.Symbol('L_1_15'), sp.Symbol('L_2_15'), sp.Symbol('L_3_15')])
Dg[10] = sp.Matrix([sp.Symbol('L_1_16'), sp.Symbol('L_2_16'), sp.Symbol('L_3_16')])
Dg[11] = sp.Matrix([sp.Symbol('L_1_17'), sp.Symbol('L_2_17'), sp.Symbol('L_3_17')])
Dg[12] = sp.Matrix([sp.Symbol('L_1_18'), 0.0                , sp.Symbol('L_3_18')])

# trunk
Dg[13] = sp.Matrix([sp.Symbol('L_1_19'), sp.Symbol('L_2_19'), sp.Symbol('L_3_19')])
Dg[14] = sp.Matrix([sp.Symbol('L_1_20'), sp.Symbol('L_2_20'), sp.Symbol('L_3_20')])
Dg[15] = sp.Matrix([sp.Symbol('L_1_21'), sp.Symbol('L_2_21'), sp.Symbol('L_3_21')])

# right arm
Dg[16] = sp.Matrix([sp.Symbol('L_1_22'), sp.Symbol('L_2_22'), sp.Symbol('L_3_22')])
Dg[17] = sp.Matrix([sp.Symbol('L_1_23'), sp.Symbol('L_2_23'), sp.Symbol('L_3_23')])
Dg[18] = sp.Matrix([sp.Symbol('L_1_24'), sp.Symbol('L_2_24'), sp.Symbol('L_3_24')])
Dg[19] = sp.Matrix([sp.Symbol('L_1_25'), sp.Symbol('L_2_25'), sp.Symbol('L_3_25')])

# left arm
Dg[20] = sp.Matrix([sp.Symbol('L_1_26'), sp.Symbol('L_2_26'), sp.Symbol('L_3_26')])
Dg[21] = sp.Matrix([sp.Symbol('L_1_27'), sp.Symbol('L_2_27'), sp.Symbol('L_3_27')])
Dg[22] = sp.Matrix([sp.Symbol('L_1_28'), sp.Symbol('L_2_28'), sp.Symbol('L_3_28')])
Dg[23] = sp.Matrix([sp.Symbol('L_1_29'), sp.Symbol('L_2_29'), sp.Symbol('L_3_29')])

# masses
M = np.array([ 'M_6', # waist
			'M_7' , 'M_8' , 'M_9' , 'M_10', 'M_11', 'M_12', # right leg
			'M_13', 'M_14', 'M_15', 'M_16', 'M_17', 'M_18', # left leg
			'M_19', 'M_20', 'M_21',   # trunk
			'M_22', 'M_23', 'M_24', 'M_25', # right arm
			'M_26', 'M_27', 'M_28', 'M_29'  # left arm
			])

# joint names
joint_id_names = np.array(['0', # waist
			'RightHipPitch_id', 'RightHipRoll_id', 'RightHipYaw_id', 'RightKneePitch_id', 'RightFootRoll_id', 'RightFootPitch_id', # right leg
			'LeftHipPitch_id' , 'LeftHipRoll_id' , 'LeftHipYaw_id' , 'LeftKneePitch_id' , 'LeftFootRoll_id' , 'LeftFootPitch_id' , # left leg
			'TorsoRoll_id'    , 'TorsoPitch_id'  , 'TorsoYaw_id'   , # trunk
			'RightShPitch_id' , 'RightShRoll_id' , 'RightShYaw_id' , 'RightElbPitch_id', # right arm
			'LeftShPitch_id'  , 'LeftShRoll_id'  , 'LeftShYaw_id'  , 'LeftElbPitch_id'   # left arm
			])

out_file_name = 'forward_kinematics'

gen_symbolic_out(out_file_name, nb_bodies, rot_axis, parent_body_index, joint_id_names, Dpt, Dg, M)

[((1288, 11, 1294, 5), 'numpy.array', 'np.array', ({(1288, 20, 1294, 4): '[0, 2, 1, 3, 2, 1, 2, 2, 1, 3, 2, 1, 2, 1, 2, 3, 2, 1, 3, 2, 2, 1, 3, 2]'}, {}), '([0, 2, 1, 3, 2, 1, 2, 2, 1, 3, 2, 1, 2, 1, 2, 3, 2, 1, 3, 2, 2, 1,\n    3, 2])', True, 'import numpy as np\n'), ((1297, 20, 1303, 5), 'numpy.array', 'np.array', ({(1297, 29, 1303, 4): '[-1, 0, 1, 2, 3, 4, 5, 0, 7, 8, 9, 10, 11, 0, 13, 14, 15, 16, 17, 18, 15, \n    20, 21, 22]'}, {}), '([-1, 0, 1, 2, 3, 4, 5, 0, 7, 8, 9, 10, 11, 0, 13, 14, 15, 16, 17, \n    18, 15, 20, 21, 22])', True, 'import numpy as np\n'), ((1311, 9, 1311, 35), 'sympy.Matrix', 'sp.Matrix', ({(1311, 19, 1311, 34): '[0.0, 0.0, 0.0]'}, {}), '([0.0, 0.0, 0.0])', True, 'import sympy as sp\n'), ((1319, 9, 1319, 35), 'sympy.Matrix', 'sp.Matrix', ({(1319, 19, 1319, 34): '[0.0, 0.0, 0.0]'}, {}), '([0.0, 0.0, 0.0])', True, 'import sympy as sp\n'), ((1327, 10, 1327, 36), 'sympy.Matrix', 'sp.Matrix', ({(1327, 20, 1327, 35): '[0.0, 0.0, 0.0]'}, {}), '([0.0, 0.0, 0.0])', True, 'import sympy as sp\n'), ((1331, 10, 1331, 36), 'sympy.Matrix', 'sp.Matrix', ({(1331, 20, 1331, 35): '[0.0, 0.0, 0.0]'}, {}), '([0.0, 0.0, 0.0])', True, 'import sympy as sp\n'), ((1386, 4, 1392, 5), 'numpy.array', 'np.array', ({(1386, 13, 1392, 4): "['M_6', 'M_7', 'M_8', 'M_9', 'M_10', 'M_11', 'M_12', 'M_13', 'M_14', 'M_15',\n    'M_16', 'M_17', 'M_18', 'M_19', 'M_20', 'M_21', 'M_22', 'M_23', 'M_24',\n    'M_25', 'M_26', 'M_27', 'M_28', 'M_29']"}, {}), "(['M_6', 'M_7', 'M_8', 'M_9', 'M_10', 'M_11', 'M_12', 'M_13',\n    'M_14', 'M_15', 'M_16', 'M_17', 'M_18', 'M_19', 'M_20', 'M_21', 'M_22',\n    'M_23', 'M_24', 'M_25', 'M_26', 'M_27', 'M_28', 'M_29'])", True, 'import numpy as np\n'), ((1395, 17, 1401, 5), 'numpy.array', 'np.array', ({(1395, 26, 1401, 4): "['0', 'RightHipPitch_id', 'RightHipRoll_id', 'RightHipYaw_id',\n    'RightKneePitch_id', 'RightFootRoll_id', 'RightFootPitch_id',\n    'LeftHipPitch_id', 'LeftHipRoll_id', 'LeftHipYaw_id',\n    'LeftKneePitch_id', 'LeftFootRoll_id', 'LeftFootPitch_id',\n    'TorsoRoll_id', 'TorsoPitch_id', 'TorsoYaw_id', 'RightShPitch_id',\n    'RightShRoll_id', 'RightShYaw_id', 'RightElbPitch_id', 'LeftShPitch_id',\n    'LeftShRoll_id', 'LeftShYaw_id', 'LeftElbPitch_id']"}, {}), "(['0', 'RightHipPitch_id', 'RightHipRoll_id', 'RightHipYaw_id',\n    'RightKneePitch_id', 'RightFootRoll_id', 'RightFootPitch_id',\n    'LeftHipPitch_id', 'LeftHipRoll_id', 'LeftHipYaw_id',\n    'LeftKneePitch_id', 'LeftFootRoll_id', 'LeftFootPitch_id',\n    'TorsoRoll_id', 'TorsoPitch_id', 'TorsoYaw_id', 'RightShPitch_id',\n    'RightShRoll_id', 'RightShYaw_id', 'RightElbPitch_id', 'LeftShPitch_id',\n    'LeftShRoll_id', 'LeftShYaw_id', 'LeftElbPitch_id'])", True, 'import numpy as np\n'), ((147, 8, 147, 78), 'numpy.array', 'np.array', ({(147, 17, 147, 77): '[[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]'}, {}), '([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])', True, 'import numpy as np\n'), ((499, 14, 499, 28), 'sympy.zeros', 'sp.zeros', ({(499, 23, 499, 24): '3', (499, 26, 499, 27): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((500, 14, 500, 28), 'sympy.zeros', 'sp.zeros', ({(500, 23, 500, 24): '3', (500, 26, 500, 27): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((502, 17, 502, 38), 'sympy.Symbol', 'sp.Symbol', ({(502, 27, 502, 37): '"""DPT_3_16"""'}, {}), "('DPT_3_16')", True, 'import sympy as sp\n'), ((503, 17, 503, 38), 'sympy.Symbol', 'sp.Symbol', ({(503, 27, 503, 37): '"""DPT_3_29"""'}, {}), "('DPT_3_29')", True, 'import sympy as sp\n'), ((737, 15, 737, 29), 'sympy.zeros', 'sp.zeros', ({(737, 24, 737, 25): '3', (737, 27, 737, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((738, 15, 738, 29), 'sympy.zeros', 'sp.zeros', ({(738, 24, 738, 25): '3', (738, 27, 738, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((931, 14, 931, 28), 'sympy.zeros', 'sp.zeros', ({(931, 23, 931, 24): '3', (931, 26, 931, 27): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((955, 14, 955, 28), 'sympy.zeros', 'sp.zeros', ({(955, 23, 955, 24): '3', (955, 26, 955, 27): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((980, 15, 980, 29), 'sympy.zeros', 'sp.zeros', ({(980, 24, 980, 25): '3', (980, 27, 980, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((999, 15, 999, 29), 'sympy.zeros', 'sp.zeros', ({(999, 24, 999, 25): '3', (999, 27, 999, 28): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((1099, 9, 1099, 23), 'sympy.zeros', 'sp.zeros', ({(1099, 18, 1099, 19): '3', (1099, 21, 1099, 22): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((1100, 9, 1100, 23), 'sympy.zeros', 'sp.zeros', ({(1100, 18, 1100, 19): '3', (1100, 21, 1100, 22): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((1140, 9, 1140, 23), 'sympy.zeros', 'sp.zeros', ({(1140, 18, 1140, 19): '3', (1140, 21, 1140, 22): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((1141, 9, 1141, 23), 'sympy.zeros', 'sp.zeros', ({(1141, 18, 1141, 19): '3', (1141, 21, 1141, 22): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((1282, 1, 1282, 19), 'os.remove', 'os.remove', ({(1282, 11, 1282, 18): 'in_temp'}, {}), '(in_temp)', False, 'import os\n'), ((509, 23, 509, 37), 'sympy.zeros', 'sp.zeros', ({(509, 32, 509, 33): '3', (509, 35, 509, 36): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((510, 23, 510, 37), 'sympy.zeros', 'sp.zeros', ({(510, 32, 510, 33): '3', (510, 35, 510, 36): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((523, 26, 523, 47), 'sympy.Symbol', 'sp.Symbol', ({(523, 36, 523, 46): '"""DPT_3_16"""'}, {}), "('DPT_3_16')", True, 'import sympy as sp\n'), ((868, 26, 868, 64), 're.split', 're.split', ({(868, 35, 868, 51): '"""([A-Z][^A-Z]*)"""', (868, 53, 868, 63): 'new_string'}, {}), "('([A-Z][^A-Z]*)', new_string)", False, 'import re\n'), ((1022, 15, 1022, 29), 'sympy.zeros', 'sp.zeros', ({(1022, 24, 1022, 25): '3', (1022, 27, 1022, 28): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((1037, 15, 1037, 29), 'sympy.zeros', 'sp.zeros', ({(1037, 24, 1037, 25): '3', (1037, 27, 1037, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((1051, 15, 1051, 29), 'sympy.zeros', 'sp.zeros', ({(1051, 24, 1051, 25): '3', (1051, 27, 1051, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((1314, 25, 1314, 45), 'sympy.Symbol', 'sp.Symbol', ({(1314, 35, 1314, 44): '"""DPT_2_2"""'}, {}), "('DPT_2_2')", True, 'import sympy as sp\n'), ((1315, 25, 1315, 45), 'sympy.Symbol', 'sp.Symbol', ({(1315, 35, 1315, 44): '"""DPT_2_6"""'}, {}), "('DPT_2_6')", True, 'import sympy as sp\n'), ((1316, 30, 1316, 50), 'sympy.Symbol', 'sp.Symbol', ({(1316, 40, 1316, 49): '"""DPT_3_8"""'}, {}), "('DPT_3_8')", True, 'import sympy as sp\n'), ((1317, 30, 1317, 51), 'sympy.Symbol', 'sp.Symbol', ({(1317, 40, 1317, 50): '"""DPT_3_10"""'}, {}), "('DPT_3_10')", True, 'import sympy as sp\n'), ((1318, 30, 1318, 51), 'sympy.Symbol', 'sp.Symbol', ({(1318, 40, 1318, 50): '"""DPT_3_12"""'}, {}), "('DPT_3_12')", True, 'import sympy as sp\n'), ((1322, 26, 1322, 46), 'sympy.Symbol', 'sp.Symbol', ({(1322, 36, 1322, 45): '"""DPT_2_3"""'}, {}), "('DPT_2_3')", True, 'import sympy as sp\n'), ((1323, 26, 1323, 47), 'sympy.Symbol', 'sp.Symbol', ({(1323, 36, 1323, 46): '"""DPT_2_18"""'}, {}), "('DPT_2_18')", True, 'import sympy as sp\n'), ((1324, 31, 1324, 52), 'sympy.Symbol', 'sp.Symbol', ({(1324, 41, 1324, 51): '"""DPT_3_20"""'}, {}), "('DPT_3_20')", True, 'import sympy as sp\n'), ((1325, 31, 1325, 52), 'sympy.Symbol', 'sp.Symbol', ({(1325, 41, 1325, 51): '"""DPT_3_22"""'}, {}), "('DPT_3_22')", True, 'import sympy as sp\n'), ((1326, 31, 1326, 52), 'sympy.Symbol', 'sp.Symbol', ({(1326, 41, 1326, 51): '"""DPT_3_24"""'}, {}), "('DPT_3_24')", True, 'import sympy as sp\n'), ((1330, 21, 1330, 41), 'sympy.Symbol', 'sp.Symbol', ({(1330, 31, 1330, 40): '"""DPT_1_4"""'}, {}), "('DPT_1_4')", True, 'import sympy as sp\n'), ((1330, 48, 1330, 68), 'sympy.Symbol', 'sp.Symbol', ({(1330, 58, 1330, 67): '"""DPT_3_4"""'}, {}), "('DPT_3_4')", True, 'import sympy as sp\n'), ((1332, 31, 1332, 52), 'sympy.Symbol', 'sp.Symbol', ({(1332, 41, 1332, 51): '"""DPT_3_32"""'}, {}), "('DPT_3_32')", True, 'import sympy as sp\n'), ((1335, 21, 1335, 42), 'sympy.Symbol', 'sp.Symbol', ({(1335, 31, 1335, 41): '"""DPT_1_36"""'}, {}), "('DPT_1_36')", True, 'import sympy as sp\n'), ((1335, 44, 1335, 65), 'sympy.Symbol', 'sp.Symbol', ({(1335, 54, 1335, 64): '"""DPT_2_36"""'}, {}), "('DPT_2_36')", True, 'import sympy as sp\n'), ((1335, 67, 1335, 88), 'sympy.Symbol', 'sp.Symbol', ({(1335, 77, 1335, 87): '"""DPT_3_36"""'}, {}), "('DPT_3_36')", True, 'import sympy as sp\n'), ((1336, 26, 1336, 47), 'sympy.Symbol', 'sp.Symbol', ({(1336, 36, 1336, 46): '"""DPT_2_39"""'}, {}), "('DPT_2_39')", True, 'import sympy as sp\n'), ((1337, 31, 1337, 52), 'sympy.Symbol', 'sp.Symbol', ({(1337, 41, 1337, 51): '"""DPT_3_41"""'}, {}), "('DPT_3_41')", True, 'import sympy as sp\n'), ((1338, 31, 1338, 52), 'sympy.Symbol', 'sp.Symbol', ({(1338, 41, 1338, 51): '"""DPT_3_43"""'}, {}), "('DPT_3_43')", True, 'import sympy as sp\n'), ((1341, 21, 1341, 42), 'sympy.Symbol', 'sp.Symbol', ({(1341, 31, 1341, 41): '"""DPT_1_37"""'}, {}), "('DPT_1_37')", True, 'import sympy as sp\n'), ((1341, 44, 1341, 65), 'sympy.Symbol', 'sp.Symbol', ({(1341, 54, 1341, 64): '"""DPT_2_37"""'}, {}), "('DPT_2_37')", True, 'import sympy as sp\n'), ((1341, 67, 1341, 88), 'sympy.Symbol', 'sp.Symbol', ({(1341, 77, 1341, 87): '"""DPT_3_37"""'}, {}), "('DPT_3_37')", True, 'import sympy as sp\n'), ((1342, 26, 1342, 47), 'sympy.Symbol', 'sp.Symbol', ({(1342, 36, 1342, 46): '"""DPT_2_46"""'}, {}), "('DPT_2_46')", True, 'import sympy as sp\n'), ((1343, 31, 1343, 52), 'sympy.Symbol', 'sp.Symbol', ({(1343, 41, 1343, 51): '"""DPT_3_48"""'}, {}), "('DPT_3_48')", True, 'import sympy as sp\n'), ((1344, 31, 1344, 52), 'sympy.Symbol', 'sp.Symbol', ({(1344, 41, 1344, 51): '"""DPT_3_50"""'}, {}), "('DPT_3_50')", True, 'import sympy as sp\n'), ((1350, 19, 1350, 37), 'sympy.Symbol', 'sp.Symbol', ({(1350, 29, 1350, 36): '"""L_1_6"""'}, {}), "('L_1_6')", True, 'import sympy as sp\n'), ((1350, 39, 1350, 57), 'sympy.Symbol', 'sp.Symbol', ({(1350, 49, 1350, 56): '"""L_2_6"""'}, {}), "('L_2_6')", True, 'import sympy as sp\n'), ((1350, 59, 1350, 77), 'sympy.Symbol', 'sp.Symbol', ({(1350, 69, 1350, 76): '"""L_3_6"""'}, {}), "('L_3_6')", True, 'import sympy as sp\n'), ((1353, 19, 1353, 37), 'sympy.Symbol', 'sp.Symbol', ({(1353, 29, 1353, 36): '"""L_1_7"""'}, {}), "('L_1_7')", True, 'import sympy as sp\n'), ((1353, 40, 1353, 58), 'sympy.Symbol', 'sp.Symbol', ({(1353, 50, 1353, 57): '"""L_2_7"""'}, {}), "('L_2_7')", True, 'import sympy as sp\n'), ((1353, 61, 1353, 79), 'sympy.Symbol', 'sp.Symbol', ({(1353, 71, 1353, 78): '"""L_3_7"""'}, {}), "('L_3_7')", True, 'import sympy as sp\n'), ((1354, 19, 1354, 37), 'sympy.Symbol', 'sp.Symbol', ({(1354, 29, 1354, 36): '"""L_1_8"""'}, {}), "('L_1_8')", True, 'import sympy as sp\n'), ((1354, 40, 1354, 58), 'sympy.Symbol', 'sp.Symbol', ({(1354, 50, 1354, 57): '"""L_2_8"""'}, {}), "('L_2_8')", True, 'import sympy as sp\n'), ((1354, 61, 1354, 79), 'sympy.Symbol', 'sp.Symbol', ({(1354, 71, 1354, 78): '"""L_3_8"""'}, {}), "('L_3_8')", True, 'import sympy as sp\n'), ((1355, 19, 1355, 37), 'sympy.Symbol', 'sp.Symbol', ({(1355, 29, 1355, 36): '"""L_1_9"""'}, {}), "('L_1_9')", True, 'import sympy as sp\n'), ((1355, 40, 1355, 58), 'sympy.Symbol', 'sp.Symbol', ({(1355, 50, 1355, 57): '"""L_2_9"""'}, {}), "('L_2_9')", True, 'import sympy as sp\n'), ((1355, 61, 1355, 79), 'sympy.Symbol', 'sp.Symbol', ({(1355, 71, 1355, 78): '"""L_3_9"""'}, {}), "('L_3_9')", True, 'import sympy as sp\n'), ((1356, 19, 1356, 38), 'sympy.Symbol', 'sp.Symbol', ({(1356, 29, 1356, 37): '"""L_1_10"""'}, {}), "('L_1_10')", True, 'import sympy as sp\n'), ((1356, 40, 1356, 59), 'sympy.Symbol', 'sp.Symbol', ({(1356, 50, 1356, 58): '"""L_2_10"""'}, {}), "('L_2_10')", True, 'import sympy as sp\n'), ((1356, 61, 1356, 80), 'sympy.Symbol', 'sp.Symbol', ({(1356, 71, 1356, 79): '"""L_3_10"""'}, {}), "('L_3_10')", True, 'import sympy as sp\n'), ((1357, 19, 1357, 38), 'sympy.Symbol', 'sp.Symbol', ({(1357, 29, 1357, 37): '"""L_1_11"""'}, {}), "('L_1_11')", True, 'import sympy as sp\n'), ((1357, 40, 1357, 59), 'sympy.Symbol', 'sp.Symbol', ({(1357, 50, 1357, 58): '"""L_2_11"""'}, {}), "('L_2_11')", True, 'import sympy as sp\n'), ((1357, 61, 1357, 80), 'sympy.Symbol', 'sp.Symbol', ({(1357, 71, 1357, 79): '"""L_3_11"""'}, {}), "('L_3_11')", True, 'import sympy as sp\n'), ((1358, 19, 1358, 38), 'sympy.Symbol', 'sp.Symbol', ({(1358, 29, 1358, 37): '"""L_1_12"""'}, {}), "('L_1_12')", True, 'import sympy as sp\n'), ((1358, 61, 1358, 80), 'sympy.Symbol', 'sp.Symbol', ({(1358, 71, 1358, 79): '"""L_3_12"""'}, {}), "('L_3_12')", True, 'import sympy as sp\n'), ((1361, 20, 1361, 39), 'sympy.Symbol', 'sp.Symbol', ({(1361, 30, 1361, 38): '"""L_1_13"""'}, {}), "('L_1_13')", True, 'import sympy as sp\n'), ((1361, 41, 1361, 60), 'sympy.Symbol', 'sp.Symbol', ({(1361, 51, 1361, 59): '"""L_2_13"""'}, {}), "('L_2_13')", True, 'import sympy as sp\n'), ((1361, 62, 1361, 81), 'sympy.Symbol', 'sp.Symbol', ({(1361, 72, 1361, 80): '"""L_3_13"""'}, {}), "('L_3_13')", True, 'import sympy as sp\n'), ((1362, 20, 1362, 39), 'sympy.Symbol', 'sp.Symbol', ({(1362, 30, 1362, 38): '"""L_1_14"""'}, {}), "('L_1_14')", True, 'import sympy as sp\n'), ((1362, 41, 1362, 60), 'sympy.Symbol', 'sp.Symbol', ({(1362, 51, 1362, 59): '"""L_2_14"""'}, {}), "('L_2_14')", True, 'import sympy as sp\n'), ((1362, 62, 1362, 81), 'sympy.Symbol', 'sp.Symbol', ({(1362, 72, 1362, 80): '"""L_3_14"""'}, {}), "('L_3_14')", True, 'import sympy as sp\n'), ((1363, 20, 1363, 39), 'sympy.Symbol', 'sp.Symbol', ({(1363, 30, 1363, 38): '"""L_1_15"""'}, {}), "('L_1_15')", True, 'import sympy as sp\n'), ((1363, 41, 1363, 60), 'sympy.Symbol', 'sp.Symbol', ({(1363, 51, 1363, 59): '"""L_2_15"""'}, {}), "('L_2_15')", True, 'import sympy as sp\n'), ((1363, 62, 1363, 81), 'sympy.Symbol', 'sp.Symbol', ({(1363, 72, 1363, 80): '"""L_3_15"""'}, {}), "('L_3_15')", True, 'import sympy as sp\n'), ((1364, 20, 1364, 39), 'sympy.Symbol', 'sp.Symbol', ({(1364, 30, 1364, 38): '"""L_1_16"""'}, {}), "('L_1_16')", True, 'import sympy as sp\n'), ((1364, 41, 1364, 60), 'sympy.Symbol', 'sp.Symbol', ({(1364, 51, 1364, 59): '"""L_2_16"""'}, {}), "('L_2_16')", True, 'import sympy as sp\n'), ((1364, 62, 1364, 81), 'sympy.Symbol', 'sp.Symbol', ({(1364, 72, 1364, 80): '"""L_3_16"""'}, {}), "('L_3_16')", True, 'import sympy as sp\n'), ((1365, 20, 1365, 39), 'sympy.Symbol', 'sp.Symbol', ({(1365, 30, 1365, 38): '"""L_1_17"""'}, {}), "('L_1_17')", True, 'import sympy as sp\n'), ((1365, 41, 1365, 60), 'sympy.Symbol', 'sp.Symbol', ({(1365, 51, 1365, 59): '"""L_2_17"""'}, {}), "('L_2_17')", True, 'import sympy as sp\n'), ((1365, 62, 1365, 81), 'sympy.Symbol', 'sp.Symbol', ({(1365, 72, 1365, 80): '"""L_3_17"""'}, {}), "('L_3_17')", True, 'import sympy as sp\n'), ((1366, 20, 1366, 39), 'sympy.Symbol', 'sp.Symbol', ({(1366, 30, 1366, 38): '"""L_1_18"""'}, {}), "('L_1_18')", True, 'import sympy as sp\n'), ((1366, 62, 1366, 81), 'sympy.Symbol', 'sp.Symbol', ({(1366, 72, 1366, 80): '"""L_3_18"""'}, {}), "('L_3_18')", True, 'import sympy as sp\n'), ((1369, 20, 1369, 39), 'sympy.Symbol', 'sp.Symbol', ({(1369, 30, 1369, 38): '"""L_1_19"""'}, {}), "('L_1_19')", True, 'import sympy as sp\n'), ((1369, 41, 1369, 60), 'sympy.Symbol', 'sp.Symbol', ({(1369, 51, 1369, 59): '"""L_2_19"""'}, {}), "('L_2_19')", True, 'import sympy as sp\n'), ((1369, 62, 1369, 81), 'sympy.Symbol', 'sp.Symbol', ({(1369, 72, 1369, 80): '"""L_3_19"""'}, {}), "('L_3_19')", True, 'import sympy as sp\n'), ((1370, 20, 1370, 39), 'sympy.Symbol', 'sp.Symbol', ({(1370, 30, 1370, 38): '"""L_1_20"""'}, {}), "('L_1_20')", True, 'import sympy as sp\n'), ((1370, 41, 1370, 60), 'sympy.Symbol', 'sp.Symbol', ({(1370, 51, 1370, 59): '"""L_2_20"""'}, {}), "('L_2_20')", True, 'import sympy as sp\n'), ((1370, 62, 1370, 81), 'sympy.Symbol', 'sp.Symbol', ({(1370, 72, 1370, 80): '"""L_3_20"""'}, {}), "('L_3_20')", True, 'import sympy as sp\n'), ((1371, 20, 1371, 39), 'sympy.Symbol', 'sp.Symbol', ({(1371, 30, 1371, 38): '"""L_1_21"""'}, {}), "('L_1_21')", True, 'import sympy as sp\n'), ((1371, 41, 1371, 60), 'sympy.Symbol', 'sp.Symbol', ({(1371, 51, 1371, 59): '"""L_2_21"""'}, {}), "('L_2_21')", True, 'import sympy as sp\n'), ((1371, 62, 1371, 81), 'sympy.Symbol', 'sp.Symbol', ({(1371, 72, 1371, 80): '"""L_3_21"""'}, {}), "('L_3_21')", True, 'import sympy as sp\n'), ((1374, 20, 1374, 39), 'sympy.Symbol', 'sp.Symbol', ({(1374, 30, 1374, 38): '"""L_1_22"""'}, {}), "('L_1_22')", True, 'import sympy as sp\n'), ((1374, 41, 1374, 60), 'sympy.Symbol', 'sp.Symbol', ({(1374, 51, 1374, 59): '"""L_2_22"""'}, {}), "('L_2_22')", True, 'import sympy as sp\n'), ((1374, 62, 1374, 81), 'sympy.Symbol', 'sp.Symbol', ({(1374, 72, 1374, 80): '"""L_3_22"""'}, {}), "('L_3_22')", True, 'import sympy as sp\n'), ((1375, 20, 1375, 39), 'sympy.Symbol', 'sp.Symbol', ({(1375, 30, 1375, 38): '"""L_1_23"""'}, {}), "('L_1_23')", True, 'import sympy as sp\n'), ((1375, 41, 1375, 60), 'sympy.Symbol', 'sp.Symbol', ({(1375, 51, 1375, 59): '"""L_2_23"""'}, {}), "('L_2_23')", True, 'import sympy as sp\n'), ((1375, 62, 1375, 81), 'sympy.Symbol', 'sp.Symbol', ({(1375, 72, 1375, 80): '"""L_3_23"""'}, {}), "('L_3_23')", True, 'import sympy as sp\n'), ((1376, 20, 1376, 39), 'sympy.Symbol', 'sp.Symbol', ({(1376, 30, 1376, 38): '"""L_1_24"""'}, {}), "('L_1_24')", True, 'import sympy as sp\n'), ((1376, 41, 1376, 60), 'sympy.Symbol', 'sp.Symbol', ({(1376, 51, 1376, 59): '"""L_2_24"""'}, {}), "('L_2_24')", True, 'import sympy as sp\n'), ((1376, 62, 1376, 81), 'sympy.Symbol', 'sp.Symbol', ({(1376, 72, 1376, 80): '"""L_3_24"""'}, {}), "('L_3_24')", True, 'import sympy as sp\n'), ((1377, 20, 1377, 39), 'sympy.Symbol', 'sp.Symbol', ({(1377, 30, 1377, 38): '"""L_1_25"""'}, {}), "('L_1_25')", True, 'import sympy as sp\n'), ((1377, 41, 1377, 60), 'sympy.Symbol', 'sp.Symbol', ({(1377, 51, 1377, 59): '"""L_2_25"""'}, {}), "('L_2_25')", True, 'import sympy as sp\n'), ((1377, 62, 1377, 81), 'sympy.Symbol', 'sp.Symbol', ({(1377, 72, 1377, 80): '"""L_3_25"""'}, {}), "('L_3_25')", True, 'import sympy as sp\n'), ((1380, 20, 1380, 39), 'sympy.Symbol', 'sp.Symbol', ({(1380, 30, 1380, 38): '"""L_1_26"""'}, {}), "('L_1_26')", True, 'import sympy as sp\n'), ((1380, 41, 1380, 60), 'sympy.Symbol', 'sp.Symbol', ({(1380, 51, 1380, 59): '"""L_2_26"""'}, {}), "('L_2_26')", True, 'import sympy as sp\n'), ((1380, 62, 1380, 81), 'sympy.Symbol', 'sp.Symbol', ({(1380, 72, 1380, 80): '"""L_3_26"""'}, {}), "('L_3_26')", True, 'import sympy as sp\n'), ((1381, 20, 1381, 39), 'sympy.Symbol', 'sp.Symbol', ({(1381, 30, 1381, 38): '"""L_1_27"""'}, {}), "('L_1_27')", True, 'import sympy as sp\n'), ((1381, 41, 1381, 60), 'sympy.Symbol', 'sp.Symbol', ({(1381, 51, 1381, 59): '"""L_2_27"""'}, {}), "('L_2_27')", True, 'import sympy as sp\n'), ((1381, 62, 1381, 81), 'sympy.Symbol', 'sp.Symbol', ({(1381, 72, 1381, 80): '"""L_3_27"""'}, {}), "('L_3_27')", True, 'import sympy as sp\n'), ((1382, 20, 1382, 39), 'sympy.Symbol', 'sp.Symbol', ({(1382, 30, 1382, 38): '"""L_1_28"""'}, {}), "('L_1_28')", True, 'import sympy as sp\n'), ((1382, 41, 1382, 60), 'sympy.Symbol', 'sp.Symbol', ({(1382, 51, 1382, 59): '"""L_2_28"""'}, {}), "('L_2_28')", True, 'import sympy as sp\n'), ((1382, 62, 1382, 81), 'sympy.Symbol', 'sp.Symbol', ({(1382, 72, 1382, 80): '"""L_3_28"""'}, {}), "('L_3_28')", True, 'import sympy as sp\n'), ((1383, 20, 1383, 39), 'sympy.Symbol', 'sp.Symbol', ({(1383, 30, 1383, 38): '"""L_1_29"""'}, {}), "('L_1_29')", True, 'import sympy as sp\n'), ((1383, 41, 1383, 60), 'sympy.Symbol', 'sp.Symbol', ({(1383, 51, 1383, 59): '"""L_2_29"""'}, {}), "('L_2_29')", True, 'import sympy as sp\n'), ((1383, 62, 1383, 81), 'sympy.Symbol', 'sp.Symbol', ({(1383, 72, 1383, 80): '"""L_3_29"""'}, {}), "('L_3_29')", True, 'import sympy as sp\n'), ((154, 10, 154, 80), 'numpy.array', 'np.array', ({(154, 19, 154, 79): '[[1.0, 0.0, 0.0], [0.0, cosine, sine], [0.0, -sine, cosine]]'}, {}), '([[1.0, 0.0, 0.0], [0.0, cosine, sine], [0.0, -sine, cosine]])', True, 'import numpy as np\n'), ((163, 10, 163, 80), 'numpy.array', 'np.array', ({(163, 19, 163, 79): '[[1.0, 0.0, 0.0], [0.0, cosine, -sine], [0.0, sine, cosine]]'}, {}), '([[1.0, 0.0, 0.0], [0.0, cosine, -sine], [0.0, sine, cosine]])', True, 'import numpy as np\n'), ((176, 10, 176, 42), 'numpy.array', 'np.array', ({(176, 19, 176, 41): '[[elem], [0.0], [0.0]]'}, {}), '([[elem], [0.0], [0.0]])', True, 'import numpy as np\n'), ((185, 10, 185, 43), 'numpy.array', 'np.array', ({(185, 19, 185, 42): '[[-elem], [0.0], [0.0]]'}, {}), '([[-elem], [0.0], [0.0]])', True, 'import numpy as np\n'), ((945, 19, 945, 39), 'sympy.Symbol', 'sp.Symbol', ({(945, 29, 945, 38): 'elem_name'}, {}), '(elem_name)', True, 'import sympy as sp\n'), ((992, 19, 992, 39), 'sympy.Symbol', 'sp.Symbol', ({(992, 29, 992, 38): 'elem_name'}, {}), '(elem_name)', True, 'import sympy as sp\n'), ((1083, 15, 1083, 29), 'sympy.zeros', 'sp.zeros', ({(1083, 24, 1083, 25): '3', (1083, 27, 1083, 28): '3'}, {}), '(3, 3)', True, 'import sympy as sp\n'), ((1084, 15, 1084, 29), 'sympy.zeros', 'sp.zeros', ({(1084, 24, 1084, 25): '3', (1084, 27, 1084, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((1085, 15, 1085, 29), 'sympy.zeros', 'sp.zeros', ({(1085, 24, 1085, 25): '3', (1085, 27, 1085, 28): '1'}, {}), '(3, 1)', True, 'import sympy as sp\n'), ((156, 10, 156, 80), 'numpy.array', 'np.array', ({(156, 19, 156, 79): '[[cosine, 0.0, -sine], [0.0, 1.0, 0.0], [sine, 0.0, cosine]]'}, {}), '([[cosine, 0.0, -sine], [0.0, 1.0, 0.0], [sine, 0.0, cosine]])', True, 'import numpy as np\n'), ((165, 10, 165, 80), 'numpy.array', 'np.array', ({(165, 19, 165, 79): '[[cosine, 0.0, sine], [0.0, 1.0, 0.0], [-sine, 0.0, cosine]]'}, {}), '([[cosine, 0.0, sine], [0.0, 1.0, 0.0], [-sine, 0.0, cosine]])', True, 'import numpy as np\n'), ((178, 10, 178, 42), 'numpy.array', 'np.array', ({(178, 19, 178, 41): '[[0.0], [elem], [0.0]]'}, {}), '([[0.0], [elem], [0.0]])', True, 'import numpy as np\n'), ((187, 10, 187, 43), 'numpy.array', 'np.array', ({(187, 19, 187, 42): '[[0.0], [-elem], [0.0]]'}, {}), '([[0.0], [-elem], [0.0]])', True, 'import numpy as np\n'), ((970, 22, 970, 42), 'sympy.Symbol', 'sp.Symbol', ({(970, 32, 970, 41): 'elem_name'}, {}), '(elem_name)', True, 'import sympy as sp\n'), ((1012, 22, 1012, 42), 'sympy.Symbol', 'sp.Symbol', ({(1012, 32, 1012, 41): 'elem_name'}, {}), '(elem_name)', True, 'import sympy as sp\n'), ((158, 10, 158, 80), 'numpy.array', 'np.array', ({(158, 19, 158, 79): '[[cosine, sine, 0.0], [-sine, cosine, 0.0], [0.0, 0.0, 1.0]]'}, {}), '([[cosine, sine, 0.0], [-sine, cosine, 0.0], [0.0, 0.0, 1.0]])', True, 'import numpy as np\n'), ((160, 10, 160, 22), 'numpy.array', 'np.array', ({(160, 19, 160, 21): '[]'}, {}), '([])', True, 'import numpy as np\n'), ((167, 10, 167, 80), 'numpy.array', 'np.array', ({(167, 19, 167, 79): '[[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]]'}, {}), '([[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]])', True, 'import numpy as np\n'), ((169, 10, 169, 22), 'numpy.array', 'np.array', ({(169, 19, 169, 21): '[]'}, {}), '([])', True, 'import numpy as np\n'), ((180, 10, 180, 42), 'numpy.array', 'np.array', ({(180, 19, 180, 41): '[[0.0], [0.0], [elem]]'}, {}), '([[0.0], [0.0], [elem]])', True, 'import numpy as np\n'), ((182, 10, 182, 22), 'numpy.array', 'np.array', ({(182, 19, 182, 21): '[]'}, {}), '([])', True, 'import numpy as np\n'), ((189, 10, 189, 43), 'numpy.array', 'np.array', ({(189, 19, 189, 42): '[[0.0], [0.0], [-elem]]'}, {}), '([[0.0], [0.0], [-elem]])', True, 'import numpy as np\n'), ((191, 10, 191, 22), 'numpy.array', 'np.array', ({(191, 19, 191, 21): '[]'}, {}), '([])', True, 'import numpy as np\n')]