def __init__(self, joint):
GenericObject.__init__(self)
children_joints = joint.get_children_joints()
for child in children_joints:
child.init_local_transformation()
p = child.localTransformation[:3, 3]
translation = p * 0.5
height = numpy.linalg.norm(p)
if height < 1e-6:
continue
radius = 0.005
new_y = p / numpy.linalg.norm(p)
new_x = numpy.cross(new_y, numpy.array([1, 0, 0]))
new_x_norm = numpy.linalg.norm(new_x)
# if new_y is colinear to numpy.array([1,0,0])
# recompute new_x
if new_x_norm < 1e-6:
new_x = numpy.cross(new_y, numpy.array([0, 1, 0]))
new_x_norm = numpy.linalg.norm(new_x)
new_x /= new_x_norm
new_z = numpy.cross(new_x, new_y)
trans_T = numpy.eye(4)
trans_T[:3, 0] = new_x
trans_T[:3, 1] = new_y
trans_T[:3, 2] = new_z
try:
angle, direc, point = tf.rotation_from_matrix(trans_T)
except:
logger.exception("Error in creating link {0}-{1}".format(
joint.name, child.name))
else:
rotation = [direc[0], direc[1], direc[2], angle]
link = Shape()
link.enabled = False
link.appearance.material.diffuseColor = [0, 0.5, 1]
link.geometry = Cylinder()
link.geometry.height = height
link.geometry.radius = radius
link.rotation = rotation
link.translation = translation
link.init()
self.add_child(link)
motor = Shape()
motor.enabled = False
self.add_child(motor)
motor.appearance.material.diffuseColor = [1, 0, 0]
if joint.jointAxis in ["X", "x", "Y", "y", "z", "Z"]:
motor.geometry = Cylinder()
motor.geometry.height = 0.005
motor.geometry.radius = 0.02
if joint.jointAxis in ["X", "x"]:
motor.rotation = [0, 0, 1, 1.5708]
elif joint.jointAxis in ["Z", "z"]:
motor.rotation = [0, 1, 0, 1.5708]
else:
motor.geometry = Sphere()
motor.geometry.radius = 0.02
def updateData(self, feature, prop):
r"""If a property of the handled feature has changed,
we have the chance to handle this here
See Also
--------
https://www.freecadweb.org/wiki/Scripted_objects
"""
debug("ViewProviderAnchor/updateData")
p = feature.getPropertyByName("p")
u = feature.getPropertyByName("u")
v = feature.getPropertyByName("v")
self.transform.translation.setValue((p[0], p[1], p[2]))
at = anchor_transformation(p0=(0, 0, 0),
u0=(0, -1, 0),
v0=(0, 0, 1),
p1=(p[0], p[1], p[2]),
u1=(u[0], u[1], u[2]),
v1=(v[0], v[1], v[2]))
t = translation_from_matrix(at)
self.transform.translation.setValue((t[0], t[1], t[2]))
angle, direction, point = rotation_from_matrix(at)
# print("angle : %f" % angle)
# print("direction : %s" % str(direction))
# print("point : %s" % str(point))
self.transform.rotation.setValue(coin.SbVec3f(direction), angle)
def writeAnimation(human, linebuffer, animTrack, config):
# TODO animations need to be adapted to rest pose and retargeted to user skeleton
import numpy as np
progress = Progress(len(human.getSkeleton().getBones()))
log.message("Exporting animation %s.", animTrack.name)
linebuffer.append(' <animation name="%s" length="%s">' % (animTrack.name, animTrack.getPlaytime()))
linebuffer.append(' <tracks>')
I = np.identity(4, dtype=np.float32)
axis_ = np.asarray([0,0,0,1], dtype=np.float32)
for bIdx, bone in enumerate(human.getSkeleton().getBones()):
# Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
linebuffer.append(' <track bone="%s">' % bone.name)
linebuffer.append(' <keyframes>')
frameTime = 1.0/float(animTrack.frameRate)
for frameIdx in xrange(animTrack.nFrames):
poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
I[:3,:4] = poseMat[:3,:4]
poseMat = I
translation = poseMat[:3,3]
angle, axis, _ = transformations.rotation_from_matrix(poseMat)
axis_[:3] = axis[:3]
axis = np.asarray(axis_ * np.matrix(bone.getRestMatrix(offsetVect=config.offset)))[0]
linebuffer.append(' <keyframe time="%s">' % (float(frameIdx) * frameTime))
linebuffer.append(' <translate x="%s" y="%s" z="%s" />' % (translation[0], translation[1], translation[2]))
# TODO account for scale
linebuffer.append(' <rotate angle="%s">' % angle)
linebuffer.append(' <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
linebuffer.append(' </rotate>')
linebuffer.append(' </keyframe>')
linebuffer.append(' </keyframes>')
linebuffer.append(' </track>')
progress.step()
linebuffer.append(' </tracks>')
linebuffer.append(' </animation>')
def writeAnimation(human, linebuffer, animTrack):
progress = Progress(len(human.getSkeleton().getBones()))
log.message("Exporting animation %s.", animTrack.name)
linebuffer.append(' <animation name="%s" length="%s">' %
(animTrack.name, animTrack.getPlaytime()))
linebuffer.append(' <tracks>')
for bIdx, bone in enumerate(human.getSkeleton().getBones()):
# Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
linebuffer.append(' <track bone="%s">' % bone.name)
linebuffer.append(' <keyframes>')
frameTime = 1.0 / float(animTrack.frameRate)
for frameIdx in xrange(animTrack.nFrames):
poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
translation = poseMat[:3, 3]
angle, axis, _ = transformations.rotation_from_matrix(poseMat)
linebuffer.append(' <keyframe time="%s">' %
(float(frameIdx) * frameTime))
linebuffer.append(
' <translate x="%s" y="%s" z="%s" />'
% (translation[0], translation[1], translation[2]))
# TODO account for scale
linebuffer.append(
' <rotate angle="%s">' % angle)
linebuffer.append(
' <axis x="%s" y="%s" z="%s" />'
% (axis[0], axis[1], axis[2]))
linebuffer.append(' </rotate>')
linebuffer.append(' </keyframe>')
linebuffer.append(' </keyframes>')
linebuffer.append(' </track>')
progress.step()
linebuffer.append(' </tracks>')
linebuffer.append(' </animation>')
def writeAnimation(human, linebuffer, animTrack, config):
import numpy as np
progress = Progress(len(human.getSkeleton().getBones()))
log.message("Exporting animation %s.", animTrack.name)
linebuffer.append(' <animation name="%s" length="%s">' % (animTrack.name, animTrack.getPlaytime()))
linebuffer.append(' <tracks>')
for bIdx, bone in enumerate(human.getSkeleton().getBones()):
# Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
linebuffer.append(' <track bone="%s">' % bone.name)
linebuffer.append(' <keyframes>')
frameTime = 1.0/float(animTrack.frameRate)
for frameIdx in xrange(animTrack.nFrames):
poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
translation = poseMat[:3,3]
angle, axis, _ = transformations.rotation_from_matrix(poseMat)
axis = np.asarray(axis * np.matrix(bone.getRestMatrix(offsetVect=config.offset)))[0]
linebuffer.append(' <keyframe time="%s">' % (float(frameIdx) * frameTime))
linebuffer.append(' <translate x="%s" y="%s" z="%s" />' % (translation[0], translation[1], translation[2]))
# TODO account for scale
linebuffer.append(' <rotate angle="%s">' % angle)
linebuffer.append(' <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
linebuffer.append(' </rotate>')
linebuffer.append(' </keyframe>')
linebuffer.append(' </keyframes>')
linebuffer.append(' </track>')
progress.step()
linebuffer.append(' </tracks>')
linebuffer.append(' </animation>')
def __init__(self, joint):
GenericObject.__init__(self)
children_joints = joint.get_children_joints()
for child in children_joints:
child.init_local_transformation()
p = child.localTransformation[:3,3]
translation = p*0.5
height = numpy.linalg.norm(p)
if height < 1e-6:
continue
radius = 0.005
new_y = p/numpy.linalg.norm(p)
new_x = numpy.cross(new_y, numpy.array([1,0,0]))
new_x_norm = numpy.linalg.norm(new_x)
# if new_y is colinear to numpy.array([1,0,0])
# recompute new_x
if new_x_norm < 1e-6:
new_x = numpy.cross(new_y, numpy.array([0,1,0]))
new_x_norm = numpy.linalg.norm(new_x)
new_x /= new_x_norm
new_z = numpy.cross(new_x, new_y)
trans_T = numpy.eye(4)
trans_T[:3,0] = new_x
trans_T[:3,1] = new_y
trans_T[:3,2] = new_z
try:
angle, direc, point = tf.rotation_from_matrix(trans_T)
except:
logger.exception("Error in creating link {0}-{1}".format(joint.name,
child.name))
else:
rotation = [direc[0], direc[1], direc[2], angle]
link = Shape()
link.enabled = False
link.appearance.material.diffuseColor = [0,0.5,1]
link.geometry = Cylinder()
link.geometry.height = height
link.geometry.radius = radius
link.rotation = rotation
link.translation = translation
link.init()
self.add_child(link)
motor = Shape()
motor.enabled = False
self.add_child(motor)
motor.appearance.material.diffuseColor = [1,0,0]
if joint.jointAxis in ["X","x","Y","y","z","Z"]:
motor.geometry = Cylinder()
motor.geometry.height = 0.005
motor.geometry.radius = 0.02
if joint.jointAxis in ["X","x"]:
motor.rotation = [0, 0, 1, 1.5708]
elif joint.jointAxis in ["Z","z"]:
motor.rotation = [0, 1, 0, 1.5708]
else:
motor.geometry = Sphere()
motor.geometry.radius = 0.02
def log_so3(R):
assert R.shape == (3, 3)
T = np.identity(4)
T[0:3, 0:3] = R
angle, axis, _ = tfs.rotation_from_matrix(T)
return angle * axis
def remove_generic_objects(self):
generic_children = [
child for child in self.children
if (type(child) == GenericObject and child.children[:])
]
while generic_children[:]:
for child in generic_children:
for grandchild in child.children[:]:
grandchild.localTransformation = numpy.dot(
child.localTransformation,
grandchild.localTransformation)
grandchild.translation = grandchild.localTransformation[:3,
3]
angle, direction, point = tf.rotation_from_matrix(
grandchild.localTransformation)
grandchild.rotation = list(direction) + [angle]
grandchild.rpy = tf.euler_from_matrix(
grandchild.localTransformation)
grandchild.scale = [
grandchild.scale[i] * child.scale[i] for i in range(3)
]
self.add_child(grandchild)
self.children.remove(child)
del child
generic_children = [
child for child in self.children
if (type(child) == GenericObject and child.children[:])
]
for child in self.children:
child.remove_generic_objects()
def update_config2(self, T, q):
self.count += 1
T = numpy.array(T)
self.localTransformation = T
self.translation = T[:3, 3]
angle, direction, point = tf.rotation_from_matrix(T)
self.rpy = tf.euler_from_matrix(T)
self.rotation = [direction[0], direction[1], direction[2], angle]
self.origin.update()
def update_config2(self, T, q):
self.count += 1
T = numpy.array(T)
self.localTransformation = T
self.translation = T[:3,3]
angle, direction, point = tf.rotation_from_matrix(T)
self.rpy = tf.euler_from_matrix(T)
self.rotation = [ direction[0],direction[1],direction[2], angle ]
self.origin.update()
def transform_pad(padcoords, pad_length, pad_separation, pad_thickness,
drum_separation, drum_radius, totalAlpha):
TranG = trn.translation_matrix((padcoords[0], padcoords[1], padcoords[2]))
if padcoords[4] != 0:
RotG = trn.rotation_matrix(padcoords[4], [0, 1, 0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(((pad_length + pad_separation), 0, 0))
if (padcoords[0] + pad_separation + pad_length) > (drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0] - pad_separation - pad_length) < -(drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -2 * math.pi:
alpha -= (totalAlpha[0] - 2 * math.pi)
totalAlpha[0] = -2 * math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else:
Final = trn.concatenate_matrices(TranG, RotG, TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3], Final[1][3], Final[2][3], 0, angle, 0]
return padcoords
def __init__(self, server = None, width = 640, height = 480):
logger.debug("Creating camera")
kinematics.GenericObject.__init__(self)
self.pixels = None
self.draw_t = 0
self.frame_seq = 0
self.server = server
self.frontClipDistance = 0.01
self.backClipDistance = 1000
self.width = width
self.height = height
self.cam_type = "COLOR"
self.fieldOfView = 45*math.pi/180
self.translation = [0, 0, 0]
self.r = 3.5
self.x0 = 0.
self.y0 = 0.
self.aspect = float(width)/float(height)
# OpenCV params
self.fx = 0.
self.fy = 0.
self.cx = float(width)/2
self.cy = float(height)/2
self.R = numpy.eye(3)
self.localTransformation[:3,:3] = numpy.array([ [ 0 , 0 , 1],
[ 1 , 0 , 0],
[ 0 , 1 , 0],
]
)
angle, direction, point = tf.rotation_from_matrix(self.localTransformation)
self.rotation = list(direction) + [angle]
self.moved = True
self.init()
self.top = math.atan(self.fovy/2)*self.near
self.bottom = -math.atan(self.fovy/2)*self.near
self.right = self.aspect*self.top
self.left = self.aspect*self.bottom
self.compute_opencv_params()
# print self.globalTransformation
self.count = 0
self.fbo = None
self.texture = None
self.render_buffer = None
self.gl_error = None
logger.debug("{0} GL vals: {1}".format(self.name,
(self.x0, self.y0,
self.fovy, self.aspect,
self.left, self.right,
self.bottom, self.top,
self.near, self.far)))
def evaluate_frame(self, frame):
head_position = self.skeleton.nodes[self.skeleton.head_joint].get_global_position(frame, use_cache=False)
target_direction = self.target_position - head_position
target_direction /= np.linalg.norm(target_direction)
head_orientation = self.skeleton.nodes[self.skeleton.head_joint].get_global_orientation_quaternion(frame, use_cache=True)
head_direction = self._get_direction_vector_from_orientation(head_orientation)
delta_q = quaternion_from_vector_to_vector(head_direction, target_direction)
angle, _, _ = rotation_from_matrix(quaternion_matrix(delta_q))
return abs(angle)
def update_config(self,config):
self.count += 1
if not config:
raise Exception("Failed to update_config for {0} with {1}".format(self.name, config))
self.translation = config[:3]
self.rpy = config[3:6]
rot_mat = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])
angle, direction, point = tf.rotation_from_matrix(rot_mat)
self.rotation = [ direction[0],direction[1],direction[2], angle ]
self.init_local_transformation()
self.origin.update()
def angle_axis(x):
"""Returns the extracted angle and axis from a valid SO3 quantity x.
This is equivalent to separating the rotvec form of x into its
magnitude and direction. The angle will always be between 0 and pi,
inclusive, and the axis will always be a unit vector.
>>> yourAngle, yourAxis = -1337, [-2, 0, 7]
>>> yourMat = trns.rotation_matrix(yourAngle, yourAxis)
>>> myAngle, myAxis = angle_axis(yourMat)
>>> print(np.isclose(myAngle, np.mod(yourAngle, 2*np.pi)))
True
>>> print(np.allclose(myAxis, normalize(yourAxis)))
True
>>> myAngle2, myAxis2 = angle_axis(trns.quaternion_from_matrix(yourMat))
>>> print(np.allclose(myAngle, myAngle2), np.allclose(myAxis, myAxis2))
(True, True)
"""
xrep = rep(x)
# In the matrix case, transformations.py already has a nice implementation:
if xrep in ['rotmat', 'tfmat']:
if xrep == 'rotmat':
x = make_affine(x)
angle, axis, point = trns.rotation_from_matrix(x)
# But to be consistent with rotvecs, we only use positive angles:
if angle < 0:
angle, axis = -angle, -axis
# And we map the identity rotation to an axis of [0, 0, 0]:
elif np.isclose(angle, 0):
axis = np.array([0, 0, 0])
# In the quaternion case, we carry out the following routine:
elif xrep == 'quaternion':
# Renormalize for accuracy:
x = normalize(x)
# If "amount of rotation" is negative, flip quaternion:
if x[0] < 0:
x = -x
# Extract axis:
axis = normalize(x[1:])
# Extract angle:
angle = 2*np.arccos(x[0])
# In the rotvec case, the routine is trivial:
elif xrep == 'rotvec':
angle = npl.norm(x)
if not np.isclose(angle, 0):
axis = x / angle
else:
axis = np.array([0, 0, 0])
# If not SO3:
else:
raise ValueError("Quantity to extract angle and axis from must be on SO3.")
# Finally:
return (angle, axis)
def update_config(self, config):
self.count += 1
if not config:
raise Exception("Failed to update_config for {0} with {1}".format(
self.name, config))
self.translation = config[:3]
self.rpy = config[3:6]
rot_mat = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])
angle, direction, point = tf.rotation_from_matrix(rot_mat)
self.rotation = [direction[0], direction[1], direction[2], angle]
self.init_local_transformation()
self.origin.update()
def transform_pad(padcoords,pad_length,pad_separation,pad_thickness,drum_separation,drum_radius,totalAlpha):
TranG = trn.translation_matrix(( padcoords[0],padcoords[1],padcoords[2] ))
if padcoords[4]!=0:
RotG = trn.rotation_matrix(padcoords[4],[0,1,0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(( (pad_length+pad_separation),0,0))
if (padcoords[0]+pad_separation+pad_length) >(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0]-pad_separation-pad_length)<-(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-2*math.pi:
alpha -= (totalAlpha[0] - 2*math.pi)
totalAlpha[0] = -2*math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else :
Final = trn.concatenate_matrices(TranG,RotG,TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3],Final[1][3],Final[2][3],0,angle,0]
return padcoords
def errors(reference, target):
if len(reference) != len(target):
raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) )
F_ref = to_homogeneous(reference)
F_tar = to_homogeneous(target)
F_ref_inv = inverse_matrix(F_ref)
T_tar_ref = concatenate_matrices(F_ref_inv, F_tar)
trans = translation_from_matrix(T_tar_ref)
ang, foo, bar = rotation_from_matrix(T_tar_ref)
return (numpy.sum( numpy.square(trans) ), ang*ang)
def load(self, di_dict, predictor):
di = DatasetItem(di_dict)
self.df['subject'] = pd.Series(data=di.get_subject(), index=di.get_stamps())
self.df['scenario'] = di.get_scenario()
self.df['humanhash'] = di.get_humanhash()
for stamp in di.get_stamps():
T_camdriver_head = di.get_T_camdriver_head(stamp)
assert T_camdriver_head is not None
T_headfrontal_head = T_headfrontal_camdriver.dot(T_camdriver_head)
self.df.at[stamp, 'gt_roll'], self.df.at[stamp, 'gt_pitch'], self.df.at[stamp, 'gt_yaw'] = tr.euler_from_matrix(T_headfrontal_head)
self.df.at[stamp, 'gt_x'], self.df.at[stamp, 'gt_y'], self.df.at[stamp, 'gt_z'] = T_camdriver_head[0:3,3]
gt_angle_from_zero, _, _ = tr.rotation_from_matrix(T_headfrontal_head)
self.df.at[stamp, 'gt_angle_from_zero'] = abs(gt_angle_from_zero)
self.df.at[stamp, 'occlusion_state'] = di.get_occlusion_state(stamp)
hypo_T_headfrontal_head = predictor.get_T_headfrontal_head(stamp)
if hypo_T_headfrontal_head is None:
self.df.at[stamp, 'hypo_roll'] = None
self.df.at[stamp, 'hypo_pitch'] = None
self.df.at[stamp, 'hypo_yaw'] = None
self.df.at[stamp, 'angle_diff'] = None
self.df.at[stamp, 'hypo_x'] = None
self.df.at[stamp, 'hypo_y'] = None
self.df.at[stamp, 'hypo_z'] = None
else:
self.df.at[stamp, 'hypo_roll'], self.df.at[stamp, 'hypo_pitch'], self.df.at[stamp, 'hypo_yaw'] = tr.euler_from_matrix(hypo_T_headfrontal_head)
angle_difference, _, _ = tr.rotation_from_matrix(tr.inverse_matrix(T_headfrontal_head).dot(hypo_T_headfrontal_head))
self.df.at[stamp, 'angle_diff'] = abs(angle_difference)
self.df.at[stamp, 'hypo_x'], self.df.at[stamp, 'hypo_y'], self.df.at[stamp, 'hypo_z'] = predictor.get_t_camdriver_head(stamp)
def scale_and_translate(Tlistgroundtruth, Tlistartoolkit):
ground_truth_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistgroundtruth])
artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistartoolkit])
Tscale = tf.affine_matrix_from_points(artoolkit_pointcloud.T,
ground_truth_pointcloud.T,
shear=False,
scale=True)
#print("Scaling by {0}".format(tf.scale_from_matrix(Tscale)[0]))
try:
print("translation by {0}".format(tf.translation_from_matrix(Tscale)))
print("rotation by {0}".format(tf.rotation_from_matrix(Tscale)[0]))
except ValueError:
pass
Tlistartoolkit = [tf.concatenate_matrices(Tscale, T) for T in Tlistartoolkit]
return Tlistartoolkit
def so3_log(r, return_angle_only=True, return_skew=False):
"""
:param r: SO(3) rotation matrix
:param return_angle_only: return only the angle (default)
:param return_skew: return skew symmetric Lie algebra element
:return: axis/angle
or if skew:
3x3 skew symmetric logarithmic map in so(3) (Ma, Soatto eq. 2.8)
"""
if not is_so3(r):
raise LieAlgebraException("matrix is not a valid SO(3) group element")
if return_angle_only and not return_skew:
return np.arccos(min(1, max(-1, (np.trace(r) - 1) / 2)))
angle, axis, _ = tr.rotation_from_matrix(se3(r, [0, 0, 0]))
if return_skew:
return hat(axis * angle)
else:
return axis, angle
def from_homogeneous(M,n):
if n != 7 and n != 3:
raise ValueError("Only poses with 3 or 7 elements supported! this one has %d" % n)
trans = translation_from_matrix(M)
if n == 7:
quat = quaternion_from_matrix(M)
out = list(trans)
out.extend(quaternion_imag(quat))
out.append(quaternion_real(quat))
else:
ang, foo, bar = rotation_from_matrix(M)
out = list(trans[0:2])
out.append(ang)
if len(out) != n:
raise ValueError("Wrong output length: %d should be %d" %(len(out), n))
return out
def remove_generic_objects(self):
generic_children = [child for child in self.children
if (type(child) == GenericObject and child.children[:])]
while generic_children[:]:
for child in generic_children:
for grandchild in child.children[:]:
grandchild.localTransformation = numpy.dot(child.localTransformation,
grandchild.localTransformation)
grandchild.translation = grandchild.localTransformation[:3,3]
angle, direction, point = tf.rotation_from_matrix(grandchild.localTransformation)
grandchild.rotation = list(direction) + [angle]
grandchild.rpy = tf.euler_from_matrix(grandchild.localTransformation)
grandchild.scale = [grandchild.scale[i]*child.scale[i] for i in range(3)]
self.add_child(grandchild)
self.children.remove(child)
del child
generic_children = [child for child in self.children
if (type(child) == GenericObject and child.children[:])]
for child in self.children:
child.remove_generic_objects()
def _post_optimization(self, grasp_contacts):
logging.info('[HFTSSampler::_post_optimization] Performing post optimization.')
transform = self._robot.GetTransform()
angle, axis, point = transformations.rotation_from_matrix(transform)
# further optimize hand configuration and pose
# TODO this is Robotiq hand specific
transform_params = axis.tolist() + [angle] + transform[:3, 3].tolist()
robot_dofs = self._robot.GetDOFValues().tolist()
def joint_limits_constraint(x, *args):
positions, normals, robot = args
lower_limits, upper_limits = robot.GetDOFLimits()
return -dist_in_range(x[0], [lower_limits[0], upper_limits[0]]) - \
dist_in_range(x[1], [lower_limits[1], upper_limits[1]])
def collision_free_constraint(x, *args):
positions, normals, robot = args
config = [x[0], x[1]]
robot.SetDOFValues(config)
env = robot.GetEnv()
links = robot.get_non_fingertip_links()
for link in links:
if env.CheckCollision(robot.GetLink(link)):
return -1.0
return 0.0
x_min = scipy.optimize.fmin_cobyla(self._post_optimization_obj_fn, robot_dofs + transform_params,
[joint_limits_constraint, collision_free_constraint],
rhobeg=.5, rhoend=1e-3,
args=(grasp_contacts[:, :3], grasp_contacts[:, 3:], self._robot),
maxfun=int(1e8), iprint=0)
self._robot.SetDOFValues(x_min[:2])
axis = x_min[2:5]
angle = x_min[5]
position = x_min[6:]
transform = transformations.rotation_matrix(angle, axis)
transform[:3, 3] = position
self._robot.SetTransform(transform)
def sketch(robot):
res = ""
res += "def robot {\n"
oldT = numpy.eye(4)
for j in robot.moving_joint_list[:20]:
T = j.globalTransformation
T1 = numpy.eye(4)
if j.jointAxis in ['y','Y']:
T1 = tf.rotation_matrix(math.pi/2, [0,0,1])
if j.jointAxis in ['z','Z']:
T1 = tf.rotation_matrix(math.pi/2, [0,1,0])
T = numpy.dot(T,T1)
relT = numpy.dot(numpy.linalg.inv(oldT),T)
angle, direc, point = tf.rotation_from_matrix(relT)
print T
p = relT[:3,3]
p = [w*10 for w in p]
res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
"""%(angle*180/math.pi,0,0,0, direc[0], direc[1], direc[2], p[0],p[1],p[2])
oldT = T
res += "}\n"
return res
def writeAnimation(human, fp, animTrack):
log.message("Exporting animation %s.", animTrack.name)
fp.write(' <animation name="%s" length="%s">\n' % (animTrack.name, animTrack.getPlaytime()))
fp.write(' <tracks>\n')
for bIdx, bone in enumerate(human.getSkeleton().getBones()):
# Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
fp.write(' <track bone="%s">\n' % bone.name)
fp.write(' <keyframes>\n')
frameTime = 1.0/float(animTrack.frameRate)
for frameIdx in xrange(animTrack.nFrames):
poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
translation = poseMat[:3,3]
angle, axis, _ = transformations.rotation_from_matrix(poseMat)
fp.write(' <keyframe time="%s">\n' % (float(frameIdx) * frameTime))
fp.write(' <translate x="%s" y="%s" z="%s" />\n' % (translation[0], translation[1], translation[2]))
# TODO account for scale
fp.write(' <rotate angle="%s">\n' % angle)
fp.write(' <axis x="%s" y="%s" z="%s" />\n' % (axis[0], axis[1], axis[2]))
fp.write(' </rotate>\n')
fp.write(' </keyframe>\n')
fp.write(' </keyframes>\n')
fp.write(' </track>\n')
fp.write(' </tracks>\n')
fp.write(' </animation>\n')
def sketch(robot):
res = ""
res += "def robot {\n"
oldT = numpy.eye(4)
for j in robot.moving_joint_list[:20]:
T = j.globalTransformation
T1 = numpy.eye(4)
if j.jointAxis in ['y', 'Y']:
T1 = tf.rotation_matrix(math.pi / 2, [0, 0, 1])
if j.jointAxis in ['z', 'Z']:
T1 = tf.rotation_matrix(math.pi / 2, [0, 1, 0])
T = numpy.dot(T, T1)
relT = numpy.dot(numpy.linalg.inv(oldT), T)
angle, direc, point = tf.rotation_from_matrix(relT)
print T
p = relT[:3, 3]
p = [w * 10 for w in p]
res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
""" % (angle * 180 / math.pi, 0, 0, 0, direc[0], direc[1], direc[2], p[0],
p[1], p[2])
oldT = T
res += "}\n"
return res
def elbow_angle(obj, light='L', heavy='H', limit_l=107, limit_h=113, draw=0):
"""
DESCRIPTION
Calculates the integer elbow angle of an antibody Fab complex and
optionally draws a graphical representation of the vectors used to
determine the angle.
ARGUMENTS
obj = string: object
light/heavy = strings: chain ID of light and heavy chains, respectively
limit_l/limit_h = integers: residue numbers of the last residue in the
light and heavy chain variable domains, respectively
draw = boolean: Choose whether or not to draw the angle visualization
REQUIRES: com.py, transformations.py, numpy (see above)
"""
# store current view
orig_view = cmd.get_view()
limit_l = int(limit_l)
limit_h = int(limit_h)
draw = int(draw)
# for temp object names
tmp_prefix = "tmp_elbow_"
prefix = tmp_prefix + obj + '_'
# names
vl = prefix + 'VL'
vh = prefix + 'VH'
cl = prefix + 'CL'
ch = prefix + 'CH'
# selections
vl_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, light,
limit_l)
vh_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, heavy,
limit_h)
cl_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, light,
limit_l)
ch_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, heavy,
limit_h)
v_sel = '((' + vl_sel + ') or (' + vh_sel + '))'
c_sel = '((' + cl_sel + ') or (' + ch_sel + '))'
# create temp objects
cmd.create(vl, vl_sel)
cmd.create(vh, vh_sel)
cmd.create(cl, cl_sel)
cmd.create(ch, ch_sel)
# superimpose vl onto vh, calculate axis and angle
Rv = calc_super_matrix(vl, vh)
angle_v, direction_v, point_v = transformations.rotation_from_matrix(Rv)
# superimpose cl onto ch, calculate axis and angle
Rc = calc_super_matrix(cl, ch)
angle_c, direction_c, point_c = transformations.rotation_from_matrix(Rc)
# delete temporary objects
cmd.delete(vl)
cmd.delete(vh)
cmd.delete(cl)
cmd.delete(ch)
# if dot product is positive, angle is acute
if (numpy.dot(direction_v, direction_c) > 0):
direction_c = direction_c * -1 # ensure angle is > 90 (need to standardize this)
# TODO: make both directions point away from the elbow axis.
elbow = int(
numpy.degrees(numpy.arccos(numpy.dot(direction_v, direction_c))))
# while (elbow < 90):
# elbow = 180 - elbow # limit to physically reasonable range
# compare the direction_v and direction_c axes to the vector defined by
# the C-alpha atoms of limit_l and limit_h of the original fab
hinge_l_sel = "%s//%s/%s/CA" % (obj, light, limit_l)
hinge_h_sel = "%s//%s/%s/CA" % (obj, heavy, limit_h)
try:
hinge_l = cmd.get_atom_coords(hinge_l_sel)
hinge_h = cmd.get_atom_coords(hinge_h_sel)
except pymol.CmdException:
# Either hinge_l_sel or hinge_h_sel atom did not exist.
raise pymol.CmdException(
'Unable to calculate elbow angle. Please check '
'your limit and chain selections and try again.')
hinge_vec = numpy.array(hinge_h) - numpy.array(hinge_l)
test = numpy.dot(hinge_vec, numpy.cross(direction_v, direction_c))
if (test > 0):
elbow = 360 - elbow
print(" Elbow angle: %i degrees" % elbow)
if (draw == 1):
# there is probably a more elegant way to do this, but
# it works so I'm not going to mess with it for now
pre = obj + '_elbow_'
# draw hinge vector
cmd.pseudoatom(pre + "hinge_l", pos=hinge_l)
cmd.pseudoatom(pre + "hinge_h", pos=hinge_h)
cmd.distance(pre + "hinge_vec", pre + "hinge_l", pre + "hinge_h")
cmd.set("dash_gap", 0)
# draw the variable domain axis
com_v = COM(v_sel)
start_v = [a - 10 * b for a, b in zip(com_v, direction_v)]
end_v = [a + 10 * b for a, b in zip(com_v, direction_v)]
cmd.pseudoatom(pre + "start_v", pos=start_v)
cmd.pseudoatom(pre + "end_v", pos=end_v)
cmd.distance(pre + "v_vec", pre + "start_v", pre + "end_v")
# draw the constant domain axis
com_c = COM(c_sel)
start_c = [a - 10 * b for a, b in zip(com_c, direction_c)]
end_c = [a + 10 * b for a, b in zip(com_c, direction_c)]
cmd.pseudoatom(pre + "start_c", pos=start_c)
cmd.pseudoatom(pre + "end_c", pos=end_c)
cmd.distance(pre + "c_vec", pre + "start_c", pre + "end_c")
# customize appearance
cmd.hide("labels", pre + "hinge_vec")
cmd.hide("labels", pre + "v_vec")
cmd.hide("labels", pre + "c_vec")
cmd.color("green", pre + "hinge_l")
cmd.color("red", pre + "hinge_h")
cmd.color("black", pre + "hinge_vec")
cmd.color("black", pre + "start_v")
cmd.color("black", pre + "end_v")
cmd.color("black", pre + "v_vec")
cmd.color("black", pre + "start_c")
cmd.color("black", pre + "end_c")
cmd.color("black", pre + "c_vec")
# draw spheres
cmd.show("spheres", pre + "hinge_l or " + pre + "hinge_h")
cmd.show("spheres", pre + "start_v or " + pre + "start_c")
cmd.show("spheres", pre + "end_v or " + pre + "end_c")
cmd.set("sphere_scale", 2)
cmd.set("dash_gap", 0, pre + "hinge_vec")
cmd.set("dash_width", 5)
cmd.set("dash_radius", 0.3)
# group drawing objects
cmd.group(pre, pre + "*")
# restore original view
cmd.set_view(orig_view)
return 0
def to_angle_axis(quaternion):
rot_mat = transf.quaternion_matrix(quaternion)
angle, direction, point = transf.rotation_from_matrix(rot_mat)
return np.array([angle, direction[0], direction[1], direction[2]])
def writeSkeletonFile(human, filepath, config):
import transformations as tm
Pprogress = Progress(3) # Parent.
filename = os.path.basename(filepath)
filename = getbasefilename(filename)
filename = filename + ".skeleton.xml"
filepath = os.path.join(os.path.dirname(filepath), filename)
skel = human.getSkeleton()
if config.scale != 1:
skel = skel.scaled(config.scale)
f = io.open(filepath, 'w', encoding="utf-8")
lines = []
lines.append('<?xml version="1.0" encoding="UTF-8"?>')
lines.append(
'<!-- Exported from MakeHuman (www.makehumancommunity.org) -->')
lines.append('<!-- Skeleton: %s -->' % skel.name)
lines.append('<skeleton>')
lines.append(' <bones>')
progress = Progress(len(skel.getBones()))
for bIdx, bone in enumerate(skel.getBones()):
mat = bone.getRelativeMatrix(
offsetVect=config.offset
) # TODO adapt offset if mesh orientation is different
# Bone positions are in parent bone space
pos = mat[:3, 3]
angle, axis, _ = tm.rotation_from_matrix(mat)
lines.append(' <bone id="%s" name="%s">' % (bIdx, bone.name))
lines.append(' <position x="%s" y="%s" z="%s" />' %
(pos[0], pos[1], pos[2]))
lines.append(' <rotation angle="%s">' % angle)
lines.append(' <axis x="%s" y="%s" z="%s" />' %
(axis[0], axis[1], axis[2]))
lines.append(' </rotation>')
lines.append(' </bone>')
progress.step()
lines.append(' </bones>')
Pprogress.step()
lines.append(' <bonehierarchy>')
progress = Progress(len(skel.getBones()))
for bone in skel.getBones():
if bone.parent:
lines.append(' <boneparent bone="%s" parent="%s" />' %
(bone.name, bone.parent.name))
progress.step()
lines.append(' </bonehierarchy>')
Pprogress.step()
animations = [human.getAnimation(name) for name in human.getAnimations()]
# TODO compensate animations for alternate rest pose
if len(animations) > 0:
lines.append(' <animations>')
for anim in animations:
# Use pose matrices, not skinning matrices
anim.resetBaked()
#anim = bvhanim.getAnimationTrack()
writeAnimation(human, lines, anim, config)
lines.append(' </animations>')
lines.append('</skeleton>')
f.write("\n".join(lines))
f.close()
Pprogress.finish()
def elbow_angle(obj, light='L', heavy='H', limit_l=107, limit_h=113, draw=0):
"""
DESCRIPTION
Calculates the integer elbow angle of an antibody Fab complex and
optionally draws a graphical representation of the vectors used to
determine the angle.
ARGUMENTS
obj = string: object
light/heavy = strings: chain ID of light and heavy chains, respectively
limit_l/limit_h = integers: residue numbers of the last residue in the
light and heavy chain variable domains, respectively
draw = boolean: Choose whether or not to draw the angle visualization
REQUIRES: com.py, transformations.py, numpy (see above)
"""
# store current view
orig_view = cmd.get_view()
limit_l = int(limit_l)
limit_h = int(limit_h)
draw = int(draw)
# for temp object names
tmp_prefix = "tmp_elbow_"
prefix = tmp_prefix + obj + '_'
# names
vl = prefix + 'VL'
vh = prefix + 'VH'
cl = prefix + 'CL'
ch = prefix + 'CH'
# selections
vl_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, light, limit_l)
vh_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, heavy, limit_h)
cl_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, light, limit_l)
ch_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, heavy, limit_h)
v_sel = '((' + vl_sel + ') or (' + vh_sel + '))'
c_sel = '((' + cl_sel + ') or (' + ch_sel + '))'
# create temp objects
cmd.create(vl, vl_sel)
cmd.create(vh, vh_sel)
cmd.create(cl, cl_sel)
cmd.create(ch, ch_sel)
# superimpose vl onto vh, calculate axis and angle
Rv = calc_super_matrix(vl, vh)
angle_v, direction_v, point_v = transformations.rotation_from_matrix(Rv)
# superimpose cl onto ch, calculate axis and angle
Rc = calc_super_matrix(cl, ch)
angle_c, direction_c, point_c = transformations.rotation_from_matrix(Rc)
# delete temporary objects
cmd.delete(vl)
cmd.delete(vh)
cmd.delete(cl)
cmd.delete(ch)
# if dot product is positive, angle is acute
if (numpy.dot(direction_v, direction_c) > 0):
direction_c = direction_c * -1 # ensure angle is > 90 (need to standardize this)
# TODO: make both directions point away from the elbow axis.
elbow = int(numpy.degrees(numpy.arccos(numpy.dot(direction_v, direction_c))))
# while (elbow < 90):
# elbow = 180 - elbow # limit to physically reasonable range
# compare the direction_v and direction_c axes to the vector defined by
# the C-alpha atoms of limit_l and limit_h of the original fab
hinge_l_sel = "%s//%s/%s/CA" % (obj, light, limit_l)
hinge_h_sel = "%s//%s/%s/CA" % (obj, heavy, limit_h)
hinge_l = cmd.get_atom_coords(hinge_l_sel)
hinge_h = cmd.get_atom_coords(hinge_h_sel)
hinge_vec = numpy.array(hinge_h) - numpy.array(hinge_l)
test = numpy.dot(hinge_vec, numpy.cross(direction_v, direction_c))
if (test > 0):
elbow = 360 - elbow
print " Elbow angle: %i degrees" % elbow
if (draw == 1):
# there is probably a more elegant way to do this, but
# it works so I'm not going to mess with it for now
pre = obj + '_elbow_'
# draw hinge vector
cmd.pseudoatom(pre + "hinge_l", pos=hinge_l)
cmd.pseudoatom(pre + "hinge_h", pos=hinge_h)
cmd.distance(pre + "hinge_vec", pre + "hinge_l", pre + "hinge_h")
cmd.set("dash_gap", 0)
# draw the variable domain axis
com_v = com.COM(v_sel)
start_v = [a - 10 * b for a, b in zip(com_v, direction_v)]
end_v = [a + 10 * b for a, b in zip(com_v, direction_v)]
cmd.pseudoatom(pre + "start_v", pos=start_v)
cmd.pseudoatom(pre + "end_v", pos=end_v)
cmd.distance(pre + "v_vec", pre + "start_v", pre + "end_v")
# draw the constant domain axis
com_c = com.COM(c_sel)
start_c = [a - 10 * b for a, b in zip(com_c, direction_c)]
end_c = [a + 10 * b for a, b in zip(com_c, direction_c)]
cmd.pseudoatom(pre + "start_c", pos=start_c)
cmd.pseudoatom(pre + "end_c", pos=end_c)
cmd.distance(pre + "c_vec", pre + "start_c", pre + "end_c")
# customize appearance
cmd.hide("labels", pre + "hinge_vec")
cmd.hide("labels", pre + "v_vec")
cmd.hide("labels", pre + "c_vec")
cmd.color("green", pre + "hinge_l")
cmd.color("red", pre + "hinge_h")
cmd.color("black", pre + "hinge_vec")
cmd.color("black", pre + "start_v")
cmd.color("black", pre + "end_v")
cmd.color("black", pre + "v_vec")
cmd.color("black", pre + "start_c")
cmd.color("black", pre + "end_c")
cmd.color("black", pre + "c_vec")
# draw spheres
cmd.show("spheres", pre + "hinge_l or " + pre + "hinge_h")
cmd.show("spheres", pre + "start_v or " + pre + "start_c")
cmd.show("spheres", pre + "end_v or " + pre + "end_c")
cmd.set("sphere_scale", 2)
cmd.set("dash_gap", 0, pre + "hinge_vec")
cmd.set("dash_width", 5)
cmd.set("dash_radius", 0.3)
# group drawing objects
cmd.group(pre, pre + "*")
# restore original view
cmd.set_view(orig_view)
return 0
def writeSkeletonFile(human, filepath, config):
import transformations as tm
Pprogress = Progress(3) # Parent.
filename = os.path.basename(filepath)
filename = getbasefilename(filename)
filename = filename + ".skeleton.xml"
filepath = os.path.join(os.path.dirname(filepath), filename)
skel = human.getSkeleton()
if config.scale != 1:
skel = skel.scaled(config.scale)
if not skel.isInRestPose():
# Export skeleton with the current pose as rest pose
skel = skel.createFromPose()
f = codecs.open(filepath, 'w', encoding="utf-8")
lines = []
lines.append('<?xml version="1.0" encoding="UTF-8"?>')
lines.append('<!-- Exported from MakeHuman (www.makehuman.org) -->')
lines.append('<!-- Skeleton: %s -->' % skel.name)
lines.append('<skeleton>')
lines.append(' <bones>')
progress = Progress(len(skel.getBones()))
for bIdx, bone in enumerate(skel.getBones()):
mat = bone.getRelativeMatrix(offsetVect=config.offset) # TODO adapt offset if mesh orientation is different
# Bone positions are in parent bone space
pos = mat[:3,3]
angle, axis, _ = tm.rotation_from_matrix(mat)
lines.append(' <bone id="%s" name="%s">' % (bIdx, bone.name))
lines.append(' <position x="%s" y="%s" z="%s" />' % (pos[0], pos[1], pos[2]))
lines.append(' <rotation angle="%s">' % angle)
lines.append(' <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
lines.append(' </rotation>')
lines.append(' </bone>')
progress.step()
lines.append(' </bones>')
Pprogress.step()
lines.append(' <bonehierarchy>')
progress = Progress(len(skel.getBones()))
for bone in skel.getBones():
if bone.parent:
lines.append(' <boneparent bone="%s" parent="%s" />' % (bone.name, bone.parent.name))
progress.step()
lines.append(' </bonehierarchy>')
Pprogress.step()
if hasattr(human, 'animations'):
lines.append(' <animations>')
for anim in human.animations:
writeAnimation(human, lines, anim.getAnimationTrack(), config)
lines.append(' </animations>')
lines.append('</skeleton>')
f.write("\n".join(lines))
f.close()
Pprogress.finish()
