def get_aligning_rotation(self, target):
start_vec = vec3(self.dual_part.x, self.dual_part.y, self.dual_part.z)
target_vec = vec3(target.dual_part.x, target.dual_part.y,
target.dual_part.z)
try:
comp_angle = start_vec.angle(target_vec)
except ValueError:
comp_angle = 0.0
#print "ValueError caught"
comp_axis = start_vec.cross(target_vec)
return quat([comp_angle, comp_axis])
def store_endpoint_data(self,target_point):
""" Storing the endpoint positions for
displaying afterwards the velocity profile.
"""
current_end_point = vec3(self.get_manipulator_coordinates()[0][3], \
self.get_manipulator_coordinates()[1][3], \
self.get_manipulator_coordinates()[2][3])
self.data_end_point_velocity.append(abs(current_end_point - self.swing_target_point))
self.data_target_distance.append(abs(current_end_point - target_point ))
self.swing_target_point = current_end_point
def __init__(self, *args):
""" Setting up the leg:
No arguments are used.
All necessary variables for the MMC computation are
initialised as list of Dual Quaternions representing the leg:
l_x - Segment translations
d_x - Diagonal translation
theta_x - joint rotation
gamma_x - diagonal rotation
delta_x - additional rotations for aligning orientations
alpha, r - target rotation and translation
For each variable exists a list which contains:
[current value,
last value,
2 or 3 different values computed from different equations]
After setup the leg is fully outstretched.
The graphic output is initialised.
"""
# Initialisation of segments as translations
self.l_0 = [dualQuaternion(),0,0,0]
self.l_1 = [dualQuaternion(),0,0,0]
self.l_2 = [dualQuaternion(),0,0,0]
if len(args)==0:
self.l_0[0].set_translation([1,0,0])
self.l_1[0].set_translation([1,0,0])
self.l_2[0].set_translation([1,0,0])
else :
self.l_0[0].set_translation([args[0][0],0,0])
self.l_1[0].set_translation([args[0][1],0,0])
self.l_2[0].set_translation([args[0][2],0,0])
# Initialisation of Diagonals and End effector Translations
self.d_0 = [self.l_0[0] * self.l_1[0],0,0,0]
self.d_1 = [self.l_1[0] * self.l_2[0],0,0,0]
self.r = [self.d_0[0] * self.l_2[0],0,0,0]
# Initialisation of Angles: all set to zero = fully stretched
self.theta_0 = [dualQuaternion(),0,0,0]
self.theta_1 = [dualQuaternion(),0,0,0]
self.theta_2 = [dualQuaternion(),0,0,0]
self.gamma_0 = [dualQuaternion(),0,0,0]
self.gamma_1 = [dualQuaternion(),0,0,0]
self.delta_0 = [dualQuaternion(),0,0,0]
self.delta_1 = [dualQuaternion(),0,0,0]
self.delta_2 = [dualQuaternion(),0,0,0]
self.alpha = [dualQuaternion(),0,0,0]
# Initialisation of Velocities and Accelerations
self.theta_0_vel = [dualQuaternion(),0,0,0]
self.theta_1_vel = [dualQuaternion(),0,0,0]
self.theta_2_vel = [dualQuaternion(),0,0,0]
self.theta_0_acc = [dualQuaternion(),0,0,0]
self.theta_1_acc = [dualQuaternion(),0,0,0]
self.theta_2_acc = [dualQuaternion(),0,0,0]
# Damping values
self.mmc_damping = 10
self.mmc_vel_damping = 0
self.mmc_acc_damping = 0
# Writing down the distance to target and the velocity
self.swing_target_point = vec3(self.get_manipulator_coordinates()[0][3], \
self.get_manipulator_coordinates()[1][3], \
self.get_manipulator_coordinates()[2][3])
self.data_end_point_velocity = []
self.data_target_distance = []
def project_onto_fixed_rotation_around_z(self, segm_dq):
if (self.real_part.toAngleAxis()[0] > 0):
rot_angle = math.atan2((self.real_part * segm_dq.dual_part * self.real_part.conjugate()).y, \
(self.real_part * segm_dq.dual_part * self.real_part.conjugate()).x )
self.real_part = quat(rot_angle, vec3(0, 0, 1))