def gen_dashstick(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=0.005,
lsolid=None,
lspace=None,
sections=8,
sticktype="rect"):
"""
:param spos: 1x3 nparray
:param epos: 1x3 nparray
:param thickness: 0.005 m by default
:param lsolid: length of the solid section, 1*thickness if None
:param lspace: length of the empty section, 1.5*thickness if None
:return:
author: weiwei
date: 20191228osaka
"""
solidweight = 1.6
spaceweight = 1.07
if not lsolid:
lsolid = thickness * solidweight
if not lspace:
lspace = thickness * spaceweight
length, direction = rm.unit_vector(epos - spos, toggle_length=True)
nstick = math.floor(length / (lsolid + lspace))
vertices = np.empty((0, 3))
faces = np.empty((0, 3))
for i in range(0, nstick):
tmp_spos = spos + (lsolid * direction + lspace * direction) * i
tmp_stick = gen_stick(spos=tmp_spos,
epos=tmp_spos + lsolid * direction,
thickness=thickness,
type=sticktype,
sections=sections)
tmp_stick_faces = tmp_stick.faces + len(vertices)
vertices = np.vstack((vertices, tmp_stick.vertices))
faces = np.vstack((faces, tmp_stick_faces))
# wrap up the last segment
tmp_spos = spos + (lsolid * direction + lspace * direction) * nstick
tmp_epos = tmp_spos + lsolid * direction
final_length, _ = rm.unit_vector(tmp_epos - spos, toggle_length=True)
if final_length > length:
tmp_epos = epos
tmp_stick = gen_stick(spos=tmp_spos,
epos=tmp_epos,
thickness=thickness,
type=sticktype,
sections=sections)
tmp_stick_faces = tmp_stick.faces + len(vertices)
vertices = np.vstack((vertices, tmp_stick.vertices))
faces = np.vstack((faces, tmp_stick_faces))
return trm.Trimesh(vertices=vertices, faces=faces)
def suction_to_with_sczy(self, gl_suction_center_pos, gl_suction_center_z, gl_suction_center_y):
"""
:param gl_suction_center_pos:
:param gl_suction_center_z: jaw_center's approaching direction
:param gl_suction_center_y: jaw_center's opening direction
:param jaw_width:
:return:
author: weiwei
date: 20220127
"""
gl_jaw_center_rotmat = np.eye(3)
gl_jaw_center_rotmat[:, 2] = rm.unit_vector(gl_suction_center_z)
gl_jaw_center_rotmat[:, 1] = rm.unit_vector(gl_suction_center_y)
gl_jaw_center_rotmat[:, 0] = np.cross(gl_jaw_center_rotmat[:3, 1], gl_jaw_center_rotmat[:3, 2])
return self.suction_to_with_scpose(gl_suction_center_pos, gl_suction_center_z)
def gen_arrow(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=0.005,
sections=8,
sticktype="rect"):
"""
:param spos: 1x3 nparray
:param epos: 1x3 nparray
:param thickness: 0.005 m by default
:param sections: # of discretized sectors used to approximate a cylinder
:param sticktype: The shape at the end of the arrow stick, round or rect
:param radius:
:return:
author: weiwei
date: 20191228osaka
"""
direction = rm.unit_vector(epos - spos)
stick = gen_stick(spos=spos,
epos=epos - direction * thickness * 4,
thickness=thickness,
type=sticktype,
sections=sections)
cap = gen_cone(spos=epos - direction * thickness * 4,
epos=epos,
radius=thickness,
sections=sections)
vertices = np.vstack((stick.vertices, cap.vertices))
capfaces = cap.faces + len(stick.vertices)
faces = np.vstack((stick.faces, capfaces))
return trm.Trimesh(vertices=vertices, faces=faces)
def drawTheCovexHull(constraints, pos=[150, 150, 150]):
print("The constraints are:",constraints)
print("The len is",len(constraints))
base.pggen.plotAxis(base.render, spos=np.array(pos), length=80, thickness=3)
convexVetices = [np.array(pos)]
convexVeticesNormal = []
for i in range(0, len(constraints)):
base.pggen.plotArrow(base.render, spos=pos,
epos=pos + constraints[i] * 40, thickness=4,
rgba=[0,0,0, 0.7])
convexVetices.append((pos + constraints[i] * 40))
convexVeticesNormal.append(constraints[i])
trimeshNew = trimesh.Trimesh(vertices=np.array(convexVetices))
try:
convexHull = trimeshNew.convex_hull
convexHullpd = pg.trimeshtonp(convexHull)
convexHullpd.setColor(0,0,0,0.2 )
convexHullpd.setTransparency(TransparencyAttrib.MAlpha)
convexHullpd.reparentTo(base.render)
except:
print("Cannot generate the convex hull")
bestDirection = np.array([0, 0, 0])
for z in constraints:
bestDirection = bestDirection + z
bestDirection = rm.unit_vector(bestDirection)
base.pggen.plotArrow(base.render, spos=pos,
epos=pos + bestDirection * 40, thickness=4,
rgba=[147.0 / 255, 112.0 / 255, 219.0 / 255, 0.7])
print("The best value is ", bestDirection)
def gen_dasharrow(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=0.005,
lsolid=None,
lspace=None,
sections=8,
sticktype="rect"):
"""
:param spos: 1x3 nparray
:param epos: 1x3 nparray
:param thickness: 0.005 m by default
:param lsolid: length of the solid section, 1*thickness if None
:param lspace: length of the empty section, 1.5*thickness if None
:return:
author: weiwei
date: 20191228osaka
"""
length, direction = rm.unit_vector(epos - spos, toggle_length=True)
cap = gen_cone(spos=epos - direction * thickness * 4,
epos=epos,
radius=thickness,
sections=sections)
dash_stick = gen_dashstick(spos=spos,
epos=epos - direction * thickness * 4,
thickness=thickness,
lsolid=lsolid,
lspace=lspace,
sections=sections,
sticktype=sticktype)
tmp_stick_faces = dash_stick.faces + len(cap.vertices)
vertices = np.vstack((cap.vertices, dash_stick.vertices))
faces = np.vstack((cap.faces, tmp_stick_faces))
return trm.Trimesh(vertices=vertices, faces=faces)
def rayhit_closet(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto - pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
contact_entry = OdeUtil.collide(ray, tgt_cdmesh, max_contacts=10)
contact_points = [
da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()
]
min_id = np.argmin(np.linalg.norm(pfrom - np.array(contact_points),
axis=1))
contact_normals = [
da.pdv3_to_npv3(contact_entry.getContactGeom(i).getNormal())
for i in range(contact_entry.getNumContacts())
]
return contact_points[min_id], contact_normals[min_id]
def grip_at_with_jczy(self, gl_jaw_center_pos, gl_jaw_center_z,
gl_jaw_center_y, jaw_width):
"""
:param gl_jaw_center_pos:
:param gl_jaw_center_z: jaw_center's approaching direction
:param gl_jaw_center_y: jaw_center's opening direction
:param jaw_width:
:return:
"""
gl_jaw_center_rotmat = np.eye(3)
gl_jaw_center_rotmat[:, 2] = rm.unit_vector(gl_jaw_center_z)
gl_jaw_center_rotmat[:, 1] = rm.unit_vector(gl_jaw_center_y)
gl_jaw_center_rotmat[:, 0] = np.cross(gl_jaw_center_rotmat[:3, 1],
gl_jaw_center_rotmat[:3, 2])
return self.grip_at_with_jcpose(gl_jaw_center_pos,
gl_jaw_center_rotmat, jaw_width)
def _extend_conf(self, conf1, conf2, ext_dist):
"""
:param conf1:
:param conf2:
:param ext_dist:
:return: a list of 1xn nparray
"""
len, vec = rm.unit_vector(conf2 - conf1, toggle_length=True)
return conf1 + ext_dist * vec if len > 1e-6 else None
def normal_from_3point(p1, p2, p3):
x1, y1, z1 = p1[0], p1[1], p1[2]
x2, y2, z2 = p2[0], p2[1], p2[2]
x3, y3, z3 = p3[0], p3[1], p3[2]
# print(x3, y3, z3)
a = (y2 - y1) * (z3 - z1) - (y3 - y1) * (z2 - z1)
b = (z2 - z1) * (x3 - x1) - (z3 - z1) * (x2 - x1)
c = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)
return rm.unit_vector(np.array([a, b, c]))
def cylinder_link_start_end(start, end, radius = 0.0003):
start = start
end = end
height = np.linalg.norm(end - start, ord = 2, axis = 0, keepdims=True)[0]
vector = rm.unit_vector(end - start)
rot3 = rm.rotmat_between_vectors(np.array([0,0,1]),vector)
rot4 = rm.homomat_from_posrot(pos = start, rot = rot3)
cylinder = cm.gen_cylinder(radius=radius, height=height, section=30, homomat=rot4)
return cylinder
def gen_dashtorus(axis=np.array([1, 0, 0]),
portion=.5,
center=np.array([0, 0, 0]),
radius=0.1,
thickness=0.005,
lsolid=None,
lspace=None,
sections=8,
discretization=24):
"""
:param axis: the circ arrow will rotate around this axis 1x3 nparray
:param portion: 0.0~1.0
:param center: the center position of the circ 1x3 nparray
:param radius:
:param thickness:
:param lsolid: length of solid
:param lspace: length of space
:param sections: # of discretized sectors used to approximate a cylindrical stick
:param discretization: number sticks used for approximating a torus
:return:
author: weiwei
date: 20200602
"""
assert (0 <= portion <= 1)
solidweight = 1.6
spaceweight = 1.07
if not lsolid:
lsolid = thickness * solidweight
if not lspace:
lspace = thickness * spaceweight
unit_axis = rm.unit_vector(axis)
starting_vector = rm.orthogonal_vector(unit_axis)
min_discretization_value = math.ceil(2 * math.pi / (lsolid + lspace))
if discretization < min_discretization_value:
discretization = min_discretization_value
nsections = math.floor(portion * 2 * math.pi * radius / (lsolid + lspace))
vertices = np.empty((0, 3))
faces = np.empty((0, 3))
for i in range(0, nsections): # TODO wrap up end
torus_sec = gen_torus(
axis=axis,
starting_vector=rm.rotmat_from_axangle(
axis,
2 * math.pi * portion / nsections * i).dot(starting_vector),
portion=portion / nsections * lsolid / (lsolid + lspace),
center=center,
radius=radius,
thickness=thickness,
sections=sections,
discretization=discretization)
torus_sec_faces = torus_sec.faces + len(vertices)
vertices = np.vstack((vertices, torus_sec.vertices))
faces = np.vstack((faces, torus_sec_faces))
return trm.Trimesh(vertices=vertices, faces=faces)
def specialMax(a,b,n):
"""
:param a: 1 by 3 np array
:param b: 1 by 3 np array
:return:
"""
crosspd = np.cross(a, b)
if np.allclose(rm.unit_vector(crosspd),n):
return a
else:
return b
def gen_rel_linear_motion_with_given_conf(self,
component_name,
goal_jnt_values,
direction,
distance,
obstacle_list=[],
granularity=0.03,
seed_jnt_values=None,
type='sink',
toggle_debug=False):
"""
:param goal_info:
:param direction:
:param distance:
:param obstacle_list:
:param granularity:
:param type: 'sink', or 'source'
:return:
author: weiwei
date: 20210114
"""
goal_tcp_pos, goal_tcp_rotmat = self.robot_s.cvt_conf_to_tcp(
component_name, goal_jnt_values)
if type == 'sink':
start_tcp_pos = goal_tcp_pos - rm.unit_vector(direction) * distance
start_tcp_rotmat = goal_tcp_rotmat
return self.gen_linear_motion(component_name,
start_tcp_pos,
start_tcp_rotmat,
goal_tcp_pos,
goal_tcp_rotmat,
obstacle_list,
granularity,
seed_jnt_values,
toggle_debug=toggle_debug)
elif type == 'source':
start_tcp_pos = goal_tcp_pos
start_tcp_rotmat = goal_tcp_rotmat
goal_tcp_pos = goal_tcp_pos + direction * distance
goal_tcp_rotmat = goal_tcp_rotmat
return self.gen_linear_motion(component_name,
start_tcp_pos,
start_tcp_rotmat,
goal_tcp_pos,
goal_tcp_rotmat,
obstacle_list,
granularity,
seed_jnt_values,
toggle_debug=toggle_debug)
else:
raise ValueError("Type must be sink or source!")
def rayhit_all(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto-pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
hit_entry = OdeUtil.collide(ray, tgt_cdmesh)
hit_points = [da.pdv3_to_npv3(point) for point in hit_entry.getContactPoints()]
hit_normals = [da.pdv3_to_npv3(hit_entry.getContactGeom(i).getNormal()) for i in range(hit_entry.getNumContacts())]
return hit_points, hit_normals
def _extend_conf(self, conf1, conf2, ext_dist, exact_end=True):
"""
:param conf1:
:param conf2:
:param ext_dist:
:return: a list of 1xn nparray
"""
len, vec = rm.unit_vector(conf2 - conf1, toggle_length=True)
# one step extension: not adopted because it is slower than full extensions, 20210523, weiwei
# return [conf1 + ext_dist * vec]
# switch to the following code for ful extensions
if not exact_end:
nval = math.ceil(len / ext_dist)
nval = 1 if nval == 0 else nval # at least include itself
conf_array = np.linspace(conf1, conf1 + nval * ext_dist * vec, nval)
else:
nval = math.floor(len / ext_dist)
nval = 1 if nval == 0 else nval # at least include itself
conf_array = np.linspace(conf1, conf1 + nval * ext_dist * vec, nval)
conf_array = np.vstack((conf_array, conf2))
return list(conf_array)
def _extend_conf(self, conf1, conf2, ext_dist):
"""
WARNING: This extend_conf is specially designed for differential-wheel robots
:param conf1:
:param conf2:
:param ext_dist:
:return: a list of 1xn nparray
author: weiwei
date: 20210530
"""
angle_ext_dist = ext_dist
len, vec = rm.unit_vector(conf2[:2] - conf1[:2], toggle_length=True)
if len > 0:
translational_theta = rm.angle_between_2d_vectors(
np.array([1, 0]), vec)
conf1_theta_to_translational_theta = translational_theta - conf1[2]
else:
conf1_theta_to_translational_theta = (conf2[2] - conf1[2])
translational_theta = conf2[2]
# rotate
nval = abs(
math.ceil(conf1_theta_to_translational_theta / angle_ext_dist))
linear_conf1 = np.array([conf1[0], conf1[1], translational_theta])
conf1_angular_arary = np.linspace(conf1, linear_conf1, nval)
# translate
nval = math.ceil(len / ext_dist)
linear_conf2 = np.array([conf2[0], conf2[1], translational_theta])
conf12_linear_arary = np.linspace(linear_conf1, linear_conf2, nval)
# rotate
translational_theta_to_conf2_theta = conf2[2] - translational_theta
nval = abs(
math.ceil(translational_theta_to_conf2_theta / angle_ext_dist))
conf2_angular_arary = np.linspace(linear_conf2, conf2, nval)
conf_array = np.vstack(
(conf1_angular_arary, conf12_linear_arary, conf2_angular_arary))
return list(conf_array)
def generate_matrix(self):
for id in self.node_infos.keys():
matrix_id_infos = {}
matrix_id_infos["parity"] = self.node_infos[id]["parity"]
if self.node_infos[id]["parity"] == "even-even":
# matrix_id_infos["top"] = self.node_infos[id]["rgt"] + np.array([-self.origin_offset, 0, 0])
# matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + np.array([self.origin_offset, 0, 0])
matrix_id_infos["top"] = self.node_infos[id]["rgt"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["rgt"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["lft"]) * self.origin_offset
matrix_id_infos["center1"] = self.node_infos[id]["up"] + np.array([0, -self.origin_offset, 0])
matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([0, 0, self.height-self.origin_offset])
matrix_id_infos["center3"] = self.node_infos[id]["down"] + np.array([0, self.origin_offset, 0])
matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([0, 0, -self.height+self.origin_offset])
matrix_id_infos["center1"] = self.node_infos[id]["up"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["up"]) * self.origin_offset
matrix_id_infos["center2"] = self.node_infos[id]["height"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["height"]) * self.origin_offset
matrix_id_infos["center3"] = self.node_infos[id]["down"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["down"]) * self.origin_offset
matrix_id_infos["center4"] = self.node_infos[id]["low"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["low"]) * self.origin_offset
elif self.node_infos[id]["parity"] == "odd-even":
# matrix_id_infos["top"] = self.node_infos[id]["rgt"] + np.array([-self.origin_offset, 0, 0])
# matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + np.array([self.origin_offset, 0, 0])
matrix_id_infos["top"] = self.node_infos[id]["rgt"] + rm.unit_vector(self.node_infos[id]["origin"] - self.node_infos[id]["rgt"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + rm.unit_vector(self.node_infos[id]["origin"] - self.node_infos[id]["lft"]) * self.origin_offset
# matrix_id_infos["center1"] = self.node_infos[id]["up"] + np.array([0, -(0.005-self.origin_offset)/1.414, (0.005-self.origin_offset)/1.414])
# matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([0, -(0.005-self.origin_offset)/1.414, (0.005-self.origin_offset)/1.414])
# matrix_id_infos["center3"] = self.node_infos[id]["down"] + np.array([0, (0.005-self.origin_offset)/1.414, -(0.005-self.origin_offset)/1.414])
# matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([0, (0.005-self.origin_offset)/1.414, -(0.005-self.origin_offset)/1.414])
matrix_id_infos["center1"] = self.node_infos[id]["origin"] + (
self.node_infos[id]["up"] - self.node_infos[id]["origin"] + np.array(
[0, -self.origin_offset, self.height - self.origin_offset])) / 1.414
matrix_id_infos["center2"] = self.node_infos[id]["origin"] + (
self.node_infos[id]["down"] - self.node_infos[id]["origin"] + np.array(
[0, self.origin_offset, self.height - self.origin_offset])) / 1.414
matrix_id_infos["center3"] = self.node_infos[id]["origin"] + (
self.node_infos[id]["down"] - self.node_infos[id]["origin"] + np.array(
[0, self.origin_offset, -self.height + self.origin_offset])) / 1.414
matrix_id_infos["center4"] = self.node_infos[id]["origin"] + (
self.node_infos[id]["up"] - self.node_infos[id]["origin"] + np.array(
[0, -self.origin_offset, -self.height + self.origin_offset])) / 1.414
elif self.node_infos[id]["parity"] == "even-odd":
# matrix_id_infos["top"] = self.node_infos[id]["up"] + np.array([0, -self.origin_offset, 0])
# matrix_id_infos["bottom"] = self.node_infos[id]["down"] + np.array([0, self.origin_offset, 0])
matrix_id_infos["top"] = self.node_infos[id]["up"] + rm.unit_vector(
self.node_infos[id]["origin"] - self.node_infos[id]["up"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id]["down"] + rm.unit_vector(
self.node_infos[id]["origin"] - self.node_infos[id]["down"]) * self.origin_offset
matrix_id_infos["center1"] = self.node_infos[id]["origin"] + (self.node_infos[id]["rgt"] - self.node_infos[id]["origin"] + np.array([-self.origin_offset, 0, self.height-self.origin_offset]))/ 1.414
matrix_id_infos["center2"] = self.node_infos[id]["origin"] + (self.node_infos[id]["lft"] - self.node_infos[id]["origin"] + np.array([self.origin_offset, 0, self.height -self.origin_offset])) / 1.414
matrix_id_infos["center3"] = self.node_infos[id]["origin"] + (self.node_infos[id]["lft"] - self.node_infos[id]["origin"] + np.array([self.origin_offset, 0, -self.height +self.origin_offset])) / 1.414
matrix_id_infos["center4"] = self.node_infos[id]["origin"] + (self.node_infos[id]["rgt"] - self.node_infos[id]["origin"] + np.array([-self.origin_offset, 0, -self.height + self.origin_offset])) / 1.414
# matrix_id_infos["center1"] = self.node_infos[id]["origin"] + np.array([-(0.005-self.origin_offset)/1.414, 0, (0.005-self.origin_offset)/1.414])
# matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([-(0.005-self.origin_offset)/1.414, 0, -(0.005-self.origin_offset)/1.414])
# matrix_id_infos["center3"] = self.node_infos[id]["origin"] + np.array([(0.005-self.origin_offset)/1.414, 0, -(0.005-self.origin_offset)/1.414])
# matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([(0.005-self.origin_offset)/1.414, 0, (0.005-self.origin_offset)/1.414])
else:
pass
self.node_matrix_infos[id] = matrix_id_infos
t_npt = cpt+direction*.07/n
# gm.gen_arrow(spos=t_npt, epos=t_npt+last_normal*.015, thickness=0.001, rgba=[1, 1, 0, 1]).attach_to(base)
nearby_sample_ids = tree.query_ball_point(t_npt, .005)
nearby_samples = bowl_samples[nearby_sample_ids]
# gm.GeometricModel(nearby_samples).attach_to(base)
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
homomat = np.eye(4)
homomat[:3,:3]=plane_rotmat
homomat[:3,3]=plane_center
# twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[.5,.7,1,.3]).attach_to(base)
projected_point = rm.project_to_plane(t_npt, plane_center, plane_normal)
# gm.gen_stick(t_npt, projected_point, thickness=.002).attach_to(base)
new_normal = rm.unit_vector(t_npt-projected_point)
if pn_direction.dot(new_normal) > .1:
new_normal = -new_normal
# gm.gen_arrow(spos=projected_point, epos=projected_point+new_normal*.015, thickness=0.001).attach_to(base)
angle = rm.angle_between_vectors(last_normal, new_normal)
vec = rm.unit_vector(np.cross(last_normal, new_normal))
new_rotmat = rm.rotmat_from_axangle(vec, angle)
direction = new_rotmat.dot(direction)
tmp_direction = new_rotmat.dot(tmp_direction)
# new_line_segs = [[cpt, cpt+direction*(.07-tick*.07/n)],
# [cpt+direction*(.07-tick*.07/n), cpt+direction*(.07-tick*.07/n)+tmp_direction*.07]]
# gm.gen_linesegs(new_line_segs).attach_to(base)
gm.gen_stick(spos=cpt, epos=projected_point, rgba=[1,.6,0,1], thickness=.002, type='round').attach_to(base)
cpt=projected_point
last_normal = new_normal
# break
bpgm.set_vert_size(.01)
bpgm1.attach_to(base)
bpgm2.attach_to(base)
lsgm = gen_linesegs([[np.array([.1, 0, .01]),
np.array([.01, 0, .01])],
[np.array([.01, 0, .01]),
np.array([.1, 0, .1])],
[np.array([.1, 0, .1]),
np.array([.1, 0, .01])]])
lsgm.attach_to(base)
gen_circarrow(radius=.1, portion=.8).attach_to(base)
gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0, 0,
2])).attach_to(base)
gen_dashframe(pos=np.array([0, 0, 0]), rotmat=np.eye(3)).attach_to(base)
axmat = rm.rotmat_from_axangle([1, 1, 1], math.pi / 4)
gen_frame(rotmat=axmat).attach_to(base)
axmat[:, 0] = .1 * axmat[:, 0]
axmat[:, 1] = .07 * axmat[:, 1]
axmat[:, 2] = .3 * axmat[:, 2]
gen_ellipsoid(pos=np.array([0, 0, 0]), axmat=axmat).attach_to(base)
print(rm.unit_vector(np.array([0, 0, 0])))
pos = np.array([.3, 0, 0])
rotmat = rm.rotmat_from_euler(math.pi / 6, 0, 0)
homomat = rm.homomat_from_posrot(pos, rotmat)
gen_frame_box([.1, .2, .3], homomat).attach_to(base)
base.run()
break
pcd_pnt_list.append(pcd_pnt)
if flag or len(pcd_pnt_list) < 3:
continue
print(pcd_pnt_list)
id_origin = 2
id_x = 0
id_minus_y = 3
for i, id in enumerate(ids):
if id == 2:
origin_xyz = pcd_pnt_list[i]
if id == 0:
plusx_xyz = pcd_pnt_list[i]
if id == 3:
minusy_xyz = pcd_pnt_list[i]
axis_x = rm.unit_vector(plusx_xyz - origin_xyz)
axis_y = rm.unit_vector(origin_xyz - minusy_xyz)
axis_z = rm.unit_vector(np.cross(axis_x, axis_y))
origin = origin_xyz
origin_rotmat = np.eye(3)
origin_rotmat[:, 0] = axis_x
origin_rotmat[:, 1] = axis_y
origin_rotmat[:, 2] = axis_z
pickle.dump([origin, origin_rotmat], open("origin.pkl", "wb"))
gm.gen_frame(pos=origin_xyz,
rotmat=origin_rotmat,
length=1,
thickness=.03).attach_to(base)
# for image_xy in image_xy_list:
# cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1)
# cv2.imshow("test", color_image)
def gen_circarrow(axis=np.array([1, 0, 0]),
starting_vector=None,
portion=0.3,
center=np.array([0, 0, 0]),
radius=0.005,
thickness=0.0015,
sections=8,
discretization=24):
"""
:param axis: the circ arrow will rotate around this axis 1x3 nparray
:param portion: 0.0~1.0
:param center: the center position of the circ 1x3 nparray
:param radius:
:param thickness:
:param rgba:
:param discretization: number sticks used for approximation
:return:
author: weiwei
date: 20200602
"""
unitaxis = rm.unit_vector(axis)
if starting_vector is None:
starting_vector = rm.orthogonal_vector(unitaxis)
else:
starting_vector = rm.unit_vector(starting_vector)
starting_pos = starting_vector * radius + center
discretizedangle = 2 * math.pi / discretization
ndist = int(portion * discretization)
# gen the last arrow first
# gen the remaining torus
if ndist > 0:
lastpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, (ndist - 1) * discretizedangle),
starting_vector) * radius
nxtpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, ndist * discretizedangle),
starting_vector) * radius
arrow = gen_arrow(spos=lastpos,
epos=nxtpos,
thickness=thickness,
sections=sections,
sticktype="round")
vertices = arrow.vertices
faces = arrow.faces
lastpos = starting_pos
for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)):
nxtpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, i * discretizedangle),
starting_vector) * radius
stick = gen_stick(spos=lastpos,
epos=nxtpos,
thickness=thickness,
sections=sections,
type="round")
stickfaces = stick.faces + len(vertices)
vertices = np.vstack((vertices, stick.vertices))
faces = np.vstack((faces, stickfaces))
lastpos = nxtpos
return trm.Trimesh(vertices=vertices, faces=faces)
else:
return trm.Trimesh()
def generate_matrix(self):
for y in range(2, self.grid.get_wid() - 2):
for x in range(2, self.grid.get_len() - 2):
id = f"{x}-{y}"
matrix_id_infos = {}
matrix_id_infos["parity"] = self.node_infos[id]["parity"]
matrix_id_infos["origin"] = self.node_infos[id]["origin"]
if self.node_infos[id]["parity"] == "even-even":
matrix_id_infos[
"top"] = self.node_infos[id]["rgt"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["rgt"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id][
"lft"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["lft"]) * self.origin_offset
matrix_id_infos["center1"] = self.node_infos[id][
"up"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["up"]) * self.origin_offset
matrix_id_infos["center2"] = self.node_infos[id][
"height"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["height"]) * self.origin_offset
matrix_id_infos["center3"] = self.node_infos[id][
"down"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["down"]) * self.origin_offset
matrix_id_infos["center4"] = self.node_infos[id][
"low"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["low"]) * self.origin_offset
elif self.node_infos[id]["parity"] == "odd-even":
matrix_id_infos[
"top"] = self.node_infos[id]["rgt"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["rgt"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id][
"lft"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["lft"]) * self.origin_offset
xy_list = id.split("-")
x = int(xy_list[0])
y = int(xy_list[1])
matrix_id_infos[
"center1"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x}-{y+1}"]["height"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center2"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x}-{y -1}"]["height"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center3"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x}-{y-1}"]["low"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center4"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x}-{y+1}"]["low"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
elif self.node_infos[id]["parity"] == "even-odd":
matrix_id_infos[
"top"] = self.node_infos[id]["up"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["up"]) * self.origin_offset
matrix_id_infos["bottom"] = self.node_infos[id][
"down"] + rm.unit_vector(
self.node_infos[id]["origin"] -
self.node_infos[id]["down"]) * self.origin_offset
xy_list = id.split("-")
x = int(xy_list[0])
y = int(xy_list[1])
matrix_id_infos[
"center1"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x + 1}-{y}"]["height"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center2"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x-1}-{y}"]["height"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center3"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x-1}-{y}"]["low"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
matrix_id_infos[
"center4"] = self.node_infos[id]["origin"] + cpt_vec(
self.node_infos[f"{x+1}-{y}"]["low"],
self.node_infos[f"{x}-{y}"]["origin"],
self.origin_offset)
else:
pass
self.node_matrix_infos[id] = matrix_id_infos
def cpt_vec(vec_a, vec_b, offset):
result_vec = (vec_a - vec_b -
rm.unit_vector(vec_a - vec_b) * offset) / 1.414
return result_vec
def checkSymbol(a,b,n):
crosspd = np.cross(a, b)
if np.allclose(rm.unit_vector(crosspd), n,atol=0.00001) or np.allclose(crosspd, np.array([0,0,0]),atol=0.00001):
return 1
else:
return -1
# cross_vec = rm.unit_vector(np.cross(np.array([0.1,0,1]), rotmat[:3,2])) cross_vec = rotmat2.dot(rotmat[:3,0]) base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) # base = wd.World(cam_pos=ax*2, lookat_pos=[0, 0, 0], toggle_debug=True) gm.gen_arrow(epos=ax*.3, rgba=[0,0,0,.3]).attach_to(base) gm.gen_frame(length=.2).attach_to(base) # gm.gen_dasharrow(epos=cross_vec*.3, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base) gm.gen_arrow(epos=cross_vec*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=cross_vec*.3, rgba=[0,0,0,1]).attach_to(base) nxt_vec_uvw = rotmat2.dot(cross_vec) gm.gen_dasharrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) # gm.gen_arrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.3, rgba=[0,0,0,1]).attach_to(base) radius, _ = rm.unit_vector(cross_vec * .3 - cross_vec.dot(ax) * ax * .3, toggle_length=True) gm.gen_arrow(spos=cross_vec.dot(ax)*ax*.3, epos = cross_vec*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_dasharrow(spos=cross_vec.dot(ax)*ax*.3, epos = nxt_vec_uvw*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_arrow(epos=ax*math.sqrt(.3**2-radius**2), rgba=[0,0,0,1]).attach_to(base) ## projections # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+np.cross(ax, cross_vec*.3)*math.sin(math.pi/6), # rgba=[1,0,1,.5]).attach_to(base) # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+(cross_vec*.3-cross_vec.dot(ax)*ax*.3)*math.cos(math.pi/6), # rgba=[0,1,1,.5]).attach_to(base) # rectangle epos_vec = rm.unit_vector(cross_vec*.3-cross_vec.dot(ax)*ax*.3) gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03,
break
projected_point = tmp_projected_point
t_npt = tmp_t_npt
domain_grid = np.meshgrid(np.linspace(-.005, .005, 100, endpoint=True),
np.linspace(-.005, .005, 100, endpoint=True))
domain_0, domain_1 = domain_grid
domain = np.column_stack(
(domain_0.ravel() + t_npt[0], domain_1.ravel() + t_npt[1]))
codomain = surface.get_zdata(domain)
vertices = np.column_stack((domain, codomain))
plane_center, plane_normal = rm.fit_plane(vertices)
new_normal = plane_normal
if pn_direction.dot(new_normal) > .1:
new_normal = -new_normal
angle = rm.angle_between_vectors(-pn_direction, new_normal)
vec = rm.unit_vector(np.cross(-pn_direction, new_normal))
new_rotmat = rm.rotmat_from_axangle(vec, angle)
direction = new_rotmat.dot(direction)
gm.gen_stick(spos=cpt,
epos=projected_point,
rgba=[1, .6, 0, 1],
thickness=.002,
type='round').attach_to(base)
cpt = projected_point
# last_normal = new_normal
direction = new_rotmat.dot(tmp_direction)
for tick in range(1, n + 1):
len = .05 / n
tmp_cpt = cpt
extended_len = 0
z_surface = surface.get_zdata(np.array([t_npt[:2]]))
projected_point = np.array([t_npt[0], t_npt[1], z_surface[0]])
domain_grid = np.meshgrid(np.linspace(-.005, .005, 100, endpoint=True),
np.linspace(-.005, .005, 100, endpoint=True))
domain_0, domain_1 = domain_grid
domain = np.column_stack(
(domain_0.ravel() + t_npt[0], domain_1.ravel() + t_npt[1]))
codomain = surface.get_zdata(domain)
vertices = np.column_stack((domain, codomain))
plane_center, plane_normal = rm.fit_plane(vertices)
new_normal = plane_normal
if pn_direction.dot(new_normal) > .1:
new_normal = -new_normal
# gm.gen_arrow(spos=projected_point, epos=projected_point+plane_normal*.015).attach_to(base)
angle = rm.angle_between_vectors(last_normal, new_normal)
vec = rm.unit_vector(np.cross(last_normal, new_normal))
new_rotmat = rm.rotmat_from_axangle(vec, angle)
direction = new_rotmat.dot(direction)
gm.gen_stick(spos=cpt,
epos=projected_point,
rgba=[1, .6, 0, 1],
thickness=.002,
type='round').attach_to(base)
tmp_len = np.linalg.norm(projected_point - cpt)
extended_len += tmp_len
cpt = projected_point
last_normal = new_normal
print(tick, extended_len)
if extended_len > .05:
break
# break
def pushpose(self, axisvec, pushpoint, toggle_debug=False):
pushposelist = []
pushpose_rotmatlist = []
zaxis = np.array([0, 0, 1])
axisvec_norm = np.linalg.norm(axisvec) ## 円錐の中心のベクトル
theta = 5
degree = 90 ## 30
handrotate = 180 ## 30
thetamax = 30 ## 60
thetarange = int(thetamax / theta)
degreerange = int(360 / degree)
handrotaterange = int(360 / handrotate)
for i in range(thetarange):
referencevec = axisvec + (axisvec_norm *
math.tan(math.radians(theta *
(i + 1)))) * zaxis
referencepoint = pushpoint + referencevec
## プッシングする点からの相対座標に変換して、クォータニオンを求める
q_refvec = Quaternion(0, referencepoint[0] - pushpoint[0],
referencepoint[1] - pushpoint[1],
referencepoint[2] - pushpoint[2])
for j in range(degreerange):
q_axis = Quaternion(axis=rm.unit_vector(axisvec),
degrees=degree * (j + 1)) ## 回転クォータニオン
q_new = q_axis * q_refvec * q_axis.inverse
## 絶対座標に戻す
point = np.array([
q_new[1] + pushpoint[0], q_new[2] + pushpoint[1],
q_new[3] + pushpoint[2]
])
# base.pggen.plotSphere(base.render, pos=point, radius=10, rgba=[0,0,1,1])
handdir = pushpoint - point
handdir_projection = copy.copy(handdir) ## xy平面への正射影
handdir_projection[2] = 0
handdir_projection = rm.unit_vector(handdir_projection)
## ハンド座標系の各要素となるベクトル
handdir = rm.unit_vector(handdir) ## z
thumb_verticalvec = np.cross(zaxis, handdir_projection) ## x
zaxis_hand = np.cross(handdir, thumb_verticalvec) ## y
# pushposelist.append(self.rtq85.approachAt(5,5,5,thumb_verticalvec[0], thumb_verticalvec[1], thumb_verticalvec[2],
# handdir[0], handdir[1], handdir[2], jawwidth=0))
## ハンドの方向を軸に-90度ずつ回転した姿勢を生成
for k in range(handrotaterange):
handrotmat = np.empty((0, 3))
## test
# handrotmat = np.append(handrotmat, np.array([handdir]), axis=0)
# handrotmat = np.append(handrotmat, np.array([thumb_verticalvec]), axis=0)
# handrotmat = np.append(handrotmat, np.array([zaxis_hand]), axis=0)
handrotmat = np.append(handrotmat,
np.array([thumb_verticalvec]),
axis=0)
handrotmat = np.append(handrotmat,
np.array([zaxis_hand]),
axis=0)
handrotmat = np.append(handrotmat,
np.array([handdir]),
axis=0)
handrotmat = handrotmat.T
handrotmat = np.dot(
rm.rotmat_from_axangle(handrotmat[:, 2],
-math.radians(handrotate * k)),
handrotmat)
pushposelist.append(
self.rtq85.grip_at_with_jczy(
np.array([.005, .005, .005]),
np.array([
handrotmat[:, 2][0], handrotmat[:, 2][1],
handrotmat[:, 2][2]
]),
np.array([
handrotmat[:, 0][0], handrotmat[:, 0][1],
handrotmat[:, 0][2]
]),
jaw_width=self.rtq85.jawwidth_rng[0]))
if toggle_debug:
self.rtq85.copy().gen_meshmodel().attach_to(base)
pushpose_rotmatlist.append(handrotmat)
return pushpose_rotmatlist
thickness=0.001,
rgba=[1, 1, 0, 1]).attach_to(base)
nearby_sample_ids = tree.query_ball_point(t_npt, .005)
nearby_samples = bowl_samples[nearby_sample_ids]
# gm.GeometricModel(nearby_samples).attach_to(base)
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
# nearby_samples_on_xy = plane_rotmat.T.dot(nearby_samples)
homomat = np.eye(4)
homomat[:3, :3] = plane_rotmat
homomat[:3, 3] = plane_center
# twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[.5,.7,1,.1]).attach_to(base)
projected_point = rm.project_to_plane(t_npt, plane_center, plane_normal)
new_normal = rm.unit_vector(t_npt - projected_point)
gm.gen_arrow(spos=projected_point,
epos=projected_point + new_normal * .015,
thickness=0.001).attach_to(base)
angle = rm.angle_between_vectors(-pn_direction, new_normal)
vec = np.cross(-pn_direction, new_normal)
new_rotmat = rm.rotmat_from_axangle(vec, angle)
direction = new_rotmat.dot(pt_direction)
# new_tmp_direction = new_rotmat.dot(tmp_direction)
new_line_segs = [[cpt, projected_point]]
# gm.gen_stick(spos=cpt, epos=t_npt, rgba=[1,0,1,1], thickness=.002, type='round').attach_to(base)
gm.gen_stick(spos=cpt,
epos=projected_point,
rgba=[1, .6, 0, 1],
thickness=.002,
type='round').attach_to(base)
