# Following code is courtesy of Michael Michalik.
# It allows transferring the inertial tensors from Meshlab to V-REP
# Refer also to this post:
# http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=2933&hilit=meshlab&start=10#p12509
import numpy as np
#Here I ask for the corresponding inertial tensor in matlab, enter it with commas seperating the numbers.
matrix=raw_input("please insert your 3x3 inertia matrix,like so 1,267.98,3.7,488,9,5,6,7,8,9.86:")
print matrix
#Here I turn the inertial tensor input into a 3 by 3 matrix by reshaping it.
matrix=np.mat(matrix)
matrix=matrix.reshape((1,3,3))
print matrix
"""Here I ask for the corresponding rotation matrix based on the relationship between meshlab axis and v-rep axis.
the relationship can be solved by looking at the meshlab center of mass and comparing it to the default v-rep center of mass"""
rotation=raw_input("please input rotation matrix:")
#By default, if you do not enter an input for the rotation matrix, it will return the identity matrix
if len(rotation)==0:
rotation=(1,0,0,0,1,0,0,0,1)
#Here I turn the rotation input into a 3 by 3 matrix by reshaping it.
rotation=np.mat(rotation)
rotation=rotation.reshape((1,3,3))
#Here I solve for the transpose of the rotational matrix
transpose=np.transpose(rotation)
print rotation
print transpose
#this function multiplies the inertial tensor by the inputted rotational matrix in order to apply the transformations
def matrix_mult(matrix,rotation):
inertia1=np.dot(rotation,matrix)
print inertia1
return inertia1
matrix_mult(matrix,rotation)
inertia2=matrix_mult(matrix,rotation)
print inertia2
#this function multiplies the result from the above function by the transpose of the rotation matrix in order to evenly distribute the mass
def matrix_mult2(inertia2,transpose):
inertia3=np.dot(inertia2,transpose)
print inertia3
return inertia3
matrix_mult2(inertia2,transpose)
final_inertia=matrix_mult2(inertia2,transpose)
inertia4=final_inertia/1000000
# V-rep requires the inertial tensor to be divided by the mass and then inputted into the program. This part divides the inertial tensor by the mass
mass=raw_input("what is the mass of your object in kilograms?")
mass2=float(mass)
final_inertia2=inertia4/mass2
print final_inertia2