def frame(self, new_frame):
# user can manually change the reference frame. If the origin is different, then the points are going to move!
assert isinstance(new_frame, trf.CoordFrame)
# this line moves the points, and explicitly specifies that the new frame was specified in world coordinates.
self.origin = trf.PointCloud(np.array([new_frame.origin]),
trf.CoordFrame())
# the orientation of the frame is simply taken from new_frame (vectors will be normalized!)
self._frame = trf.CoordFrame(i=new_frame.i,
j=new_frame.j,
k=new_frame.k,
origin=self.origin)
def frame(self, new_frame):
# User can manually change the reference frame. Points follow frame.
assert isinstance(new_frame, trf.CoordFrame)
new_frame = trf.CoordFrame(
new_frame.m) # to ensure new_frame matrix has unit vectors vectors
# The relative positions of the points don't change because the points are following the frame
self.pts = trf.PointCloud(self.pts.in_frame(self.frame).co, new_frame)
self._frame = new_frame
def origin(self, new_origin):
# moving the origin for this type of mesh will move the points!
assert isinstance(new_origin, trf.PointCloud)
tfmat = np.array(
mathutils.Matrix.Translation(new_origin.in_world().co[0, :] -
self.origin))
new_frame = trf.CoordFrame(tfmat @ self.frame.m)
self.frame = new_frame
def normal(self, new_normal):
"""new_normal is a 'direction' in world coordinates."""
assert isinstance(new_normal, trf.PointCloud)
# trf.normal2tfmat takes a unit vector (or normalizes it) and computes a transformation matrix required to transform the point (0, 0, 1) to nhat
n2tf = trf.m4(
trf.normal2tfmat(new_normal.in_frame(self.frame).co[0, :]))
# the new frame of reference is the transformed coordinate system pushed into the world.
new_frame = trf.CoordFrame(self.frame.m @ n2tf)
self.frame = new_frame
def frame(self):
# ensure i, j, k are unit vectors, and origin is at the center of the points
k_vec = self.normal
pts_in_world = self.pts.in_world()
x_vec = pts_in_world.co[0, :] - pts_in_world.center.co[0, :]
j_vec = np.cross(k_vec, x_vec)
i_vec = np.cross(j_vec, k_vec)
return trf.CoordFrame(i=i_vec,
j=j_vec,
k=k_vec,
origin=self.origin,
unit_vectors=True)
def _transform_bones(bones, articulation_center_rel,
articulation_center_frames, articulation_angle):
"""
:param bones: (list of bpn.core.MeshObject) list of bones to be transformed
:param articulation_center_rel: (3-element np array) location of the center of transformation. The position of the rotation center does not change in this frame.
:param articulation_center_frames: (list of CoordFrames, same length as number of keyframes)
The coordinate frame in which articulation center is specified (specifies the frame in world coordinates)
:param articulation angle: (radians) how much to articulate that joint
"""
for tkf in range(1, n_keyframes):
articulation_center = trf.PointCloud(
articulation_center_rel,
articulation_center_frames[tkf]).in_world().co[0, :]
progress = (tkf / n_keyframes)**1
q = trf.Quat([1, 0, 0], articulation_angle * progress,
trf.CoordFrame(origin=articulation_center))
for b in bones:
all_frames[b.name][tkf] = q * all_frames[b.name][tkf]
def __init__(self, parent, **kwargs):
"""
:param parent: (core.MeshObject)
:param vi: (1D int32 numpy array) indices of parent vertices
:param ei: (1D int32 numpy array) indices of parent edges
:param fi: (1D int32 numpy array) indices of parent faces
"""
kwargs_def = {
'vi': [],
'ei': [],
'fi': [],
'tags': [],
'call_stack': None,
'frame': None
}
kwargs_alias = {
'vi': ['vi', 'v_idx'],
'ei': ['ei', 'e_idx'],
'fi': ['fi', 'f_idx'],
'tags': ['tags'],
'call_stack': ['call_stack'],
'frame': ['frame', 'coord_frame', 'm']
}
kwargs_curr, _ = pn.clean_kwargs(kwargs, kwargs_def, kwargs_alias)
self.parent = parent
self.vi = kwargs_curr['vi']
self.ei = kwargs_curr['ei']
self.fi = kwargs_curr['fi']
self.tags = kwargs_curr['tags']
self.call_stack = kwargs_curr['call_stack']
if not kwargs_curr['frame']:
self._frame = self.parent.frame
else:
if not isinstance(kwargs_curr['frame'], trf.CoordFrame):
self._frame = trf.CoordFrame(m=kwargs_curr['frame'])
else:
self._frame = kwargs_curr['frame']
self._frame = self.frame # run the getter! This might seem trivial for this class, but look at the inherited classes and it should make sense
c.key(est_frames, 'theta', np.pi - 15 * np.pi / 180)
c.key(1, 'fov', 90)
c.key(est_frames, 'fov', 37)
c.key(1, 'center', (0, 0, 1))
c.key(est_frames, 'center', (0, 0, 0.4))
c.key(1, 'target', (0, 0, 1))
c.key(est_frames, 'target', (0, 0, 0.25))
keyframe_offset = est_frames
nutations_in, nutations_out = nutations.load_nutation_coordframes()
nutation_trf_out2in = {}
nutation_trf_in2out = {}
for bone_name in nutations_in:
nutation_trf_out2in[bone_name] = trf.CoordFrame(
nutations_in[bone_name].m @ np.linalg.inv(nutations_out[bone_name].m))
nutation_trf_in2out[bone_name] = trf.CoordFrame(
nutations_out[bone_name].m @ np.linalg.inv(nutations_in[bone_name].m))
if 'Skeletal_Sys' not in [c.name for c in bpy.data.collections]:
SKELETON = "D:\\Dropbox (MIT)\\Anatomy\\Workspace\\Ultimate_Human_Anatomy_Rigged_Blend_2-81\\skeletalSystem_originAtCenter_bkp02.blend"
anatomy.load_coll('Skeletal_Sys', SKELETON)
n_keyframes = 22
n_keyframes_nutation = 20
n_keyframes_hold = 10
hip_articulation_center_world = (-0.10529670119285583, 0.007387260906398296,
0.9325617551803589)
knee_articulation_center_world = (-0.10663662105798721, 0.03164456784725189,
0.5140388011932373)
def frame(self):
# ensure i, j, k are unit vectors, and origin is at the center of the points
return trf.CoordFrame(i=self._frame.i,
j=self._frame.j,
k=self._frame.k,
origin=self.origin)
def capsule():
"""Capsule for saving to URDF"""
import os
name = "capsule01"
rel_dir_name = "robots"
sav_dir = str(Path(utils.PATH["cache"]).parent.joinpath(rel_dir_name))
sav_file = ''.join([sav_dir, os.sep, name])
# create the capsule using the bpn.new module
r = 0.25
h = 2
s1 = new.sphere(name, r=r, v=9)
tmp1 = s1.v
# elongate the lower half
tmp1[tmp1[:, -1] < -0.001,
-1] = tmp1[tmp1[:, -1] < -0.001, -1] - (h - 2 * r)
s1.v = tmp1
s1.vertex_center = np.array([0, 0, 0])
# save STL file
# Note that the STL file does not 'bake' the pose information in
# Specify that info in the URDF file (3 times - for inertia, visuals and collisions)
s1.export(sav_file + ".stl")
# pose, mass, inertia tensor, mesh file name -> URDF file
import urdfpy as u
def make_link(link_name, mesh_name, mesh, density, pose, scale=None):
if scale is None:
scale = [1, 1, 1]
mesh.density = density
geometry = u.Geometry(
mesh=u.Mesh(mesh_name, meshes=[mesh], scale=scale))
inertia = u.Inertial(mesh.mass, mesh.moment_inertia, pose)
visuals = [u.Visual(geometry, origin=pose)]
collisions = [u.Collision(link_name + "_coll", pose, geometry)]
return u.Link(link_name, inertia, visuals, collisions)
import trimesh
st = trimesh.load_mesh(sav_file + ".stl")
links = []
# For the first link, create a transformation matrix as you would in blender
s1.frame = trf.CoordFrame(origin=[0, 0, -0.5 * (h + r)])
s1.frame = trf.Quat([1, 0, 0], np.pi / 4, trf.CoordFrame()) * s1.frame
links.append(make_link("left", name + ".stl", st, 10, s1.frame.m))
# create the joint before creating the second link!
j2 = new.empty("left_right")
j2.frame = trf.Quat([1, 0, 0], -np.pi / 4, j2.frame) * j2.frame
# the second link (child in joint 2) will have its coordinate frame defined in j2
s2 = s1.deepcopy()
s2.frame = trf.CoordFrame(origin=[0, 0, -0.5 * (h + r)])
links.append(
make_link("right", name + ".stl", st, 1, s2.frame.m, [1, 1, 1]))
joints = [
u.Joint("left_right",
"continuous",
"left",
"right",
axis=[1, 0, 0],
origin=j2.frame.m)
]
r = u.URDF(name, links, joints)
r.save(sav_file + ".urdf")
# add friction and restitution information to the URDF file
from lxml import etree
tree = etree.parse(sav_file + ".urdf",
etree.XMLParser(remove_blank_text=True))
root = tree.getroot()
for link in root:
if link.tag == 'link':
contact = etree.SubElement(link, "contact")
etree.SubElement(contact, "restitution").set("value", "1.0")
etree.SubElement(contact, "rolling_friction").set("value", "0.001")
etree.SubElement(contact,
"spinning_friction").set("value", "0.001")
f = open(sav_file + ".urdf", "w")
f.write(
etree.tostring(root, pretty_print=True,
xml_declaration=True).decode("utf-8"))
f.close()