def gen_linear_motion(self,
component_name,
start_tcp_pos,
start_tcp_rotmat,
goal_tcp_pos,
goal_tcp_rotmat,
obstacle_list=[],
granularity=0.03,
seed_jnt_values=None,
toggle_debug=False):
"""
:param component_name:
:param start_tcp_pos:
:param start_tcp_rotmat:
:param goal_tcp_pos:
:param goal_tcp_rotmat:
:param goal_info:
:param obstacle_list:
:param granularity:
:return:
author: weiwei
date: 20210125
"""
jnt_values_bk = self.robot_s.get_jnt_values(component_name)
pos_list, rotmat_list = rm.interplate_pos_rotmat(
start_tcp_pos,
start_tcp_rotmat,
goal_tcp_pos,
goal_tcp_rotmat,
granularity=granularity)
jnt_values_list = []
if seed_jnt_values is None:
seed_jnt_values = jnt_values_bk
for (pos, rotmat) in zip(pos_list, rotmat_list):
jnt_values = self.robot_s.ik(component_name,
pos,
rotmat,
seed_jnt_values=seed_jnt_values)
if jnt_values is None:
print("IK not solvable in gen_linear_motion!")
self.robot_s.fk(component_name, jnt_values_bk)
return None
else:
self.robot_s.fk(component_name, jnt_values)
cd_result, ct_points = self.robot_s.is_collided(
obstacle_list, toggle_contact_points=True)
if cd_result:
if toggle_debug:
for ct_pnt in ct_points:
gm.gen_sphere(ct_pnt).attach_to(base)
print("Intermediate pose collided in gen_linear_motion!")
self.robot_s.fk(component_name, jnt_values_bk)
return None
jnt_values_list.append(jnt_values)
seed_jnt_values = jnt_values
self.robot_s.fk(component_name, jnt_values_bk)
return jnt_values_list
def gen_colps(self, radius=30, max_smp=120, show=False):
nsample = int(math.ceil(self.objcm.trimesh.area / (radius**2 / 3.0)))
if nsample > max_smp:
nsample = max_smp
samples = self.objcm.sample_surface_even(self.objcm.trimesh, nsample)
samples = pcdu.trans_pcd(samples, self.objmat4)
if show:
for p in samples:
gm.gen_sphere(pos=p, rgba=(1, 1, 0, .2), radius=radius)
return samples
def gen_mesh_model(self,
radius=.005,
rgba=np.array([1, 0, 0, 1])) -> mc.ModelCollection:
"""
Plot markers for rigid body
"""
markers_mc = mc.ModelCollection()
for i in range(len(self)):
gm.gen_sphere(self.markers[i], radius=radius,
rgba=rgba).attach_to(markers_mc)
return markers_mc
def showCoodinate(self, id):
axis1 = self.coordinate[2*id]
# print("check1")
# print(axis1, "11")
gm.gen_mycframe(rotmat=axis1, pos=self.origin[2*id], thickness=0.01).attach_to(base)
# axis2 = self.coordinate[2*id+1]
# gm.gen_frame(rotmat=axis2).attach_to(base)
print(self.endpairs[2*id][0],"cccc")
# gm.gen_sphere(pos=self.endpairs[2*id][0], radius=0.0051).attach_to(base)
# gm.gen_sphere(pos=self.endpairs[2 * id][1], radius=0.0051).attach_to(base)
# gm.gen_sphere(pos=self.temporigin[2 * id], radius=0.01).attach_to(base)
gm.gen_sphere(pos=self.origin[2 * id], radius=0.01).attach_to(base)
def showfeatures(self, node, normal, placementfacetpairscenter,
placementVertices, placementCoM, showCoM, showNormal,
showVertices):
np.random.seed(0)
rgb = np.random.rand(3)
# color = (random.random(),random.random(),random.random(),1)
color = (rgb[0], rgb[1], rgb[2], 1)
# length = random.randint(60,100)
length = 0.050
# self.__showSptPnt(self.holdingpairsSptPnt[i], color=color)
if showNormal == True:
for i in range(len(normal)):
gm.gen_arrow(
spos=np.array([
placementfacetpairscenter[i][0],
placementfacetpairscenter[i][1],
placementfacetpairscenter[i][2]
]),
epos=np.array([
placementfacetpairscenter[i][0] +
length * normal[i][0],
placementfacetpairscenter[i][1] +
length * normal[i][1],
placementfacetpairscenter[i][2] + length * normal[i][2]
]),
thickness=0.005,
rgba=color).attach_to(node)
if showCoM == True:
gm.gen_sphere(pos=(placementCoM[0], placementCoM[1],
placementCoM[2]),
radius=.003,
rgba=color).attach_to(node)
gm.gen_arrow(spos=np.array(
[placementCoM[0], placementCoM[1], placementCoM[2]]),
epos=np.array([
placementCoM[0], placementCoM[1],
placementCoM[2] - 0.100
]),
thickness=0.001,
rgba=color).attach_to(node)
if showVertices == True:
for vertices in placementVertices:
color2 = (random.random(), random.random(), random.random(), 1)
for i in range(len(vertices)):
gm.gen_sphere(pos=(vertices[i][0], vertices[i][1],
vertices[i][2]),
radius=.005,
rgba=color2).attach_to(node)
def show(self, thickness = 0.0003):
for y in range(self.wid_num):
for x in range(self.len_num):
gm.gen_sphere(pos = self.position_matrix[y][x], radius = 0.0006, rgba=(0,0,0,1)).attach_to(base)
if x == self.len_num - 1 and y < self.wid_num - 1:
print("check")
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y + 1],
thickness=thickness).attach_to(base)
elif y == self.wid_num - 1 and x < self.len_num-1:
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x + 1][y],
thickness=thickness).attach_to(base)
elif x < self.len_num-1 and y < self.wid_num - 1:
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x+1][y], thickness=thickness).attach_to(
base)
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y+1], thickness=thickness).attach_to(
base)
def gen_endsphere(self, rgba=None, name=''):
"""
generate an end sphere (es) to show the trajectory of the end effector
:param jlobject: a JntLnk object
:param rbga: color of the arm
:return: null
author: weiwei
date: 20181003madrid, 20200331
"""
eesphere = gm.StaticGeometricModel(name=name)
if rgba is not None:
gm.gen_sphere(pos=self.jlobject.joints[-1]['linkend'],
radius=.025,
rgba=rgba).attach_to(eesphere)
return gm.StaticGeometricModel(eesphere)
def is_mesh_collided(self, objcm_list=[], toggle_debug=False):
for i, cdelement in enumerate(self.all_cdelements):
pos = cdelement['gl_pos']
rotmat = cdelement['gl_rotmat']
self.cdmesh_collection.cm_list[i].set_pos(pos)
self.cdmesh_collection.cm_list[i].set_rotmat(rotmat)
iscollided, collided_points = self.cdmesh_collection.cm_list[i].is_mcdwith(objcm_list, True)
if iscollided:
if toggle_debug:
print(self.cdmesh_collection.cm_list[i].get_homomat())
self.cdmesh_collection.cm_list[i].show_cdmesh()
for objcm in objcm_list:
objcm.show_cdmesh()
for point in collided_points:
import modeling.geometric_model as gm
gm.gen_sphere(point, radius=.001).attach_to(base)
print("collided")
return True
return False
def gen_colps_top(self, show=False):
col_ps = []
ps = self.pcd
x_range = (min([x[0] for x in ps]), max([x[0] for x in ps]))
y_range = (min([x[1] for x in ps]), max([x[1] for x in ps]))
step = 10
for i in range(int(x_range[0]), int(x_range[1]) + 1, step):
for j in range(int(y_range[0]), int(y_range[1]) + 1, step):
ps_temp = np.asarray([
p for p in ps
if i < p[0] < i + step and j < p[1] < j + step
])
if len(ps_temp) != 0:
p = ps_temp[ps_temp[:,
2] == max([x[2] for x in ps_temp])][0]
col_ps.append(p)
col_ps = np.asarray(col_ps)
if show:
for p in col_ps:
gm.gen_sphere(pos=p, rgba=(1, 0, 0, 1), radius=20)
return col_ps
def gen_sphere(pos=np.array([0, 0, 0]), radius=0.01, rgba=[1, 0, 0, 1]):
"""
:param pos:
:param radius:
:param rgba:
:return:
author: weiwei
date: 20161212tsukuba, 20191228osaka
"""
sphere_sgm = gm.gen_sphere(pos=pos, radius=radius, rgba=rgba)
sphere_cm = CollisionModel(sphere_sgm)
return sphere_cm
def showmultiface(self, base, num):
length = 0.02
for i, facet in enumerate(self.pFaces[num]):
color = (random.random(), random.random(), random.random(), 1)
for j, face in enumerate(facet):
# color = (random.random(), random.random(), random.random(), 1)
gm.gen_sphere(face[0][0:3], radius=.005,
rgba=color).attach_to(base)
gm.gen_sphere(face[1][0:3], radius=.005,
rgba=color).attach_to(base)
gm.gen_sphere(face[2][0:3], radius=.005,
rgba=color).attach_to(base)
hufunc.drawanySingleSurface(base,
vertices=np.array(face),
color=color)
gm.gen_arrow(spos=np.array([
self.pFacetpairscenter[num][i][0],
self.pFacetpairscenter[num][i][1],
self.pFacetpairscenter[num][i][2]
]),
epos=np.array([
self.pFacetpairscenter[num][i][0] +
length * self.pNormals[num][i][0],
self.pFacetpairscenter[num][i][1] +
length * self.pNormals[num][i][1],
self.pFacetpairscenter[num][i][2] +
length * self.pNormals[num][i][2]
]),
thickness=0.005,
rgba=color)
def update(pk_obj, pcd_list, marker_center_list, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
if len(marker_center_list) != 0:
for marker_center in marker_center_list:
marker_center.detach()
marker_center_list.clear()
pk_obj.device_get_capture()
color_image_handle = pk_obj.capture_get_color_image()
depth_image_handle = pk_obj.capture_get_depth_image()
if color_image_handle and depth_image_handle:
color_image = pk_obj.image_convert_to_numpy(color_image_handle)
point_cloud = pk_obj.transform_depth_image_to_point_cloud(
depth_image_handle)
parameters = aruco.DetectorParameters_create()
color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
color_image,
dictionary=aruco.Dictionary_get(aruco.DICT_4X4_250),
parameters=parameters)
if len(corners) == 0:
return task.cont
image_xy = np.mean(np.mean(corners[0], axis=0),
axis=0).astype(np.int16)
pcd_pnt = pk_obj.transform_color_xy_to_pcd_xyz(
input_color_image_handle=color_image_handle,
input_depth_image_handle=depth_image_handle,
color_xy=image_xy)
# cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1)
# cv2.imshow("test", color_image)
# cv2.waitKey(0)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1)
marker_center.attach_to(base)
marker_center_list.append(marker_center)
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
return task.cont
def update(pk_obj, pcd_list, ball_center_list, counter, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
# if len(ball_center_list) != 0:
# for ball_center in ball_center_list:
# ball_center.detach()
# ball_center_list.clear()
while True:
pk_obj.device_get_capture()
color_image_handle = pk_obj.capture_get_color_image()
depth_image_handle = pk_obj.capture_get_depth_image()
if color_image_handle and depth_image_handle:
break
point_cloud = pk_obj.transform_depth_image_to_point_cloud(
depth_image_handle)
point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud)
ball = []
for id, point_cloud_sub in enumerate(point_cloud):
if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[
1] < .3 and 0.5 < point_cloud_sub[2] < 2.5:
ball.append(point_cloud_sub)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
if len(ball) > 20:
center = np.mean(np.array(ball), axis=0)
ball_center_list.append([center, counter[0]])
ball = gm.gen_sphere(center, radius=.1)
ball.attach_to(base)
ball_list.append(ball)
if len(ball_center_list) > 2:
x = []
y = []
z = []
t = []
for cen, f_id in ball_center_list:
x.append(cen[0])
y.append(cen[1])
z.append(cen[2])
t.append(f_id)
para_x = np.polyfit(t, x, 1)
para_y = np.polyfit(t, y, 1)
para_z = np.polyfit(t, z, 2)
f_x = np.poly1d(para_x)
f_y = np.poly1d(para_y)
f_z = np.poly1d(para_z)
orbit = []
for t in np.linspace(ball_center_list[0][1],
ball_center_list[0][1] + 5, 100):
orbit.append(np.array([f_x(t), f_y(t), f_z(t)]))
for id in range(len(orbit)):
if id > 0:
tmp_stick = gm.gen_stick(spos=orbit[id - 1],
epos=orbit[id],
thickness=.01,
type="round")
tmp_stick.attach_to(base)
para_list.append(tmp_stick)
return task.done
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
counter[0] += 1
return task.cont
robot.fk(hand_name, middle_conf_list[-1])
robot.jaw_to(hand_name, 0.03)
robot_meshmodel = robot.gen_meshmodel()
robot_meshmodel.attach_to(base)
objb_copy = object.copy()
objb_copy.set_rgba([0, 191 / 255, 1, 1])
objb_copy.set_homomat(objpose_list[-1])
objb_copy.attach_to(base)
for i in range(0, len(conf_list), 2):
robot.fk(hand_name, conf_list[i])
if jawwidth_list[i] < 0.04:
robot.jaw_to(hand_name, jawwidth_list[i])
robot_meshmodel = robot.gen_meshmodel()
tcp = robot.get_gl_tcp(hand_name)
gm.gen_sphere(tcp[0], radius=0.005).attach_to(base)
# robot_meshmodel.attach_to(base)
# robot_attached_list.append(robot_meshmodel)
obj_pose = objpose_list[i]
objb_copy = object.copy()
objb_copy.set_rgba([0, 191 / 255, 1, 0.08])
objb_copy.set_homomat(obj_pose)
objb_copy.attach_to(base)
# object_attached_list.append(objb_copy)
base.run()
# print(len(conf_list), conf_list)
# if conf_list is None:
# exit(-1)
robot_attached_list = []
object_attached_list = []
def update(objbm, comlist, count, countforrot, time, objposnode, error,
estimation, estimation_com, clock, task):
# print(time[0])
if time[0] == 1:
clock[0] = tm.time()
error[0] = geterrorconfiguration(errorrange_xyz, errorrange_rpy)
# objbm[0] = bdm.BDModel(objpath, mass=1000, restitution=restitution,dynamic=True, friction=.3, shapetype="triangle")
objbm[0].set_homomat(
computehomomat(RotMatnozero[countforrot[0]],
heightlist[count[0]],
error=error[0]))
objbm[0].set_linearVelocity(0)
objbm[0].set_angularVelocity(0)
if time[0] == 10:
objbm[0].set_mass(1)
# base.attach_internal_update_obj(objbm[0])
objposnode[0] = printonscreen(
pos=(-1, 0.4, 0),
words="Initial ObjPos (Real): x=" +
str(0.600 + np.round(error[0][0], decimals=5)) + " " + "y=" +
str(0 + np.round(error[0][1], decimals=5)) + " " + "z=" +
str(heightlist[count[0]] + np.round(error[0][1], decimals=5)))
objposnode[1] = printonscreen(pos=(-1, -0.2, 0),
words="Repetition: " +
str(count[1] + 1) + "/" +
str(repetition))
objposnode[4] = printonscreen(
pos=(-1, 0.3, 0),
words="Initial ObjPos (Ideal): x=" + str(0.600) + " " + "y=" +
str(0) + " " + "z=" + str(heightlist[count[0]]))
objposnode[5] = printonscreen(
pos=(-1, 0.2, 0),
words="Rot error: x axis: " +
str(np.round(error[0][3], decimals=5)) + " " + "y axis: " +
str(np.round(error[0][4], decimals=2)) + " " + "z axis: " +
str(np.rad2deg(np.round(error[0][5], decimals=5))) +
" (degree)")
objposnode[6] = printonscreen(pos=(-0.5, 0.8, 0),
words=str(countforrot[0]),
color="navy")
# estimation_com[1] = NodePath("normal")
if time[0] == 320:
estimation_com[0] = NodePath("normal")
gm.gen_sphere(pos=np.round(np.dot(computehomomatforcom(),
comlist[countforrot[0]]),
decimals=4)[:3],
radius=.0030,
rgba=(1, 0, 0, 0.5)).attach_to(estimation_com[0])
gm.gen_sphere(pos=np.round(np.dot(
objbm[0].get_homomat(),
np.concatenate((objbm[0].cm.objtrm.center_mass, np.array([1])),
axis=0)),
decimals=4)[:3],
radius=.0030,
rgba=(0, 1, 0, 0.5)).attach_to(estimation_com[0])
estimation_com[0].reparentTo(base.render)
# estimation_com[0].reparentTo(base.render)
if time[0] == 399:
if objposnode[3] is not None:
objposnode[3].detachNode()
if time[0] == 400:
objposnode[0].detachNode()
objposnode[1].detachNode()
objposnode[4].detachNode()
objposnode[5].detachNode()
objposnode[6].detachNode()
estimation_com[0].detachNode()
# estimation_com[1].detachNode()
objcom = np.round(np.dot(
objbm[0].get_homomat(),
np.concatenate((objbm[0].cm.objtrm.center_mass, np.array([1])),
axis=0)),
decimals=5)
com = np.round(np.dot(computehomomatforcom(),
comlist[countforrot[0]]),
decimals=5)
print(np.linalg.norm(objcom - com))
# < 0.0001
if objcom[0] <= com[0] + 0.0001 and objcom[0] >= com[0] - 0.0001 \
and objcom[1] <= com[1] + 0.0001 and objcom[1] >= com[1] - 0.0001 \
and objcom[2] <= com[2] + 0.0001 and objcom[2] >= com[2] - 0.0001:
objposnode[3] = printonscreen(pos=(-0.65, 0.1, 0),
words="Stable",
color="green")
criteria = 1
else:
objposnode[3] = printonscreen(pos=(-0.65, 0.1, 0),
words="Unstable!!",
color="red")
criteria = 0
estimation.append([
countforrot[0], RotMatnozeroID[countforrot[0]], error[0],
[.600, 0, heightlist[count[0]]], objcom, criteria
])
objbm[0].set_rgba((.3, .5, .7, 0.5))
clock[1] = tm.time()
print(clock[1] - clock[0])
if count[1] == repetition - 1:
count[1] = 0
count[0] += 1
else:
count[1] += 1
if count[0] == len(heightlist):
countforrot[0] += 1
# print("Placement No.",countforrot[0])
count[0] = 0
count[1] = 0
if countforrot[0] == len(RotMatnozero):
# if countforrot[0] == len(RotMatnozero):
name = objname + "_estimation_3.pickle"
with open(name, "wb") as file:
pickle.dump(estimation, file)
time[0] = 0
# if base.inputmgr.keymap['space'] is True:
# objbm[0]=bdm.BDModel(objpath, mass=1, restitution=restitution, dynamic=True, friction=.2, type="triangles")
# error[0] = geterrorconfiguration(errorrange_xyz, errorrange_rpy)
# objbm[0].sethomomat(computehomomat(RotMatnozero[countforrot[0]], heightlist[count[0]], error=error[0]))
# objbm[0].setColor(.3, .5, .7, 0.2)
# base.attach_internal_update_obj(objbm[0])
time[0] += 1
return task.cont
objcm1 = cm.CollisionModel(objpath)
homomat = np.eye(4)
homomat[:3, :3] = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2)
homomat[:3, 3] = np.array([0.02, 0.02, 0])
objcm1.set_homomat(homomat)
objcm1.set_rgba([1, 1, .3, .2])
objcm2 = objcm1.copy()
# objcm2= cm.gen_stick(thickness=.07)
# objcm2.set_rgba([1, 0, 1, .1])
objcm2.set_pos(objcm1.get_pos() + np.array([.03, .0, .0]))
# objcm1.change_cdmesh_type('convex_hull')
# objcm2.change_cdmesh_type('obb')
iscollided, contact_points = is_collided(objcm1.cdmesh, objcm2.cdmesh)
objcm1.show_cdmesh()
objcm2.show_cdmesh()
objcm1.attach_to(base)
objcm2.attach_to(base)
print(iscollided)
for ctpt in contact_points:
gm.gen_sphere(ctpt, radius=.001).attach_to(base)
# pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0])
# pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -0.9])
# rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm2)
# objcm.attach_to(base)
# objcm.show_cdmesh(type='box')
# objcm.show_cdmesh(type='convex_hull')
# gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base)
# gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base)
# gm.gen_arrow(spos=hitpos, epos=hitpos + hitnrml * .07, thickness=.002, rgba=np.array([0, 1, 0, 1])).attach_to(base)
base.run()
# hnd_s
# contact_pairs, contact_points = gpa.plan_contact_pairs(object_bunny,
# max_samples=10000,
# min_dist_between_sampled_contact_points=.014,
# angle_between_contact_normals=math.radians(160),
# toggle_sampled_points=True)
# for p in contact_points:
# gm.gen_sphere(p, radius=.002).attach_to(base)
# base.run()
# pickle.dump(contact_pairs, open( "save.p", "wb" ))
contact_pairs = pickle.load(open("save.p", "rb"))
for i, cp in enumerate(contact_pairs):
contact_p0, contact_n0 = cp[0]
contact_p1, contact_n1 = cp[1]
rgba = rm.get_rgba_from_cmap(i)
gm.gen_sphere(contact_p0, radius=.002, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p0,
contact_p0 + contact_n0 * .01,
thickness=.0012,
rgba=rgba).attach_to(base)
# gm.gen_arrow(contact_p0, contact_p0-contact_n0*.1, thickness=.0012, rgba = rgba).attach_to(base)
gm.gen_sphere(contact_p1, radius=.002, rgba=rgba).attach_to(base)
# gm.gen_dashstick(contact_p0, contact_p1, thickness=.0012, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p1,
contact_p1 + contact_n1 * .01,
thickness=.0012,
rgba=rgba).attach_to(base)
# gm.gen_dasharrow(contact_p1, contact_p1+contact_n1*.03, thickness=.0012, rgba=rgba).attach_to(base)
# base.run()
gripper_s = rtq85.Robotiq85()
contact_offset = .002
import numpy as np rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat) rotmat2 = rm.rotmat_from_axangle(ax, math.pi/6) # 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),
thickness=.001,
rgba=[0, 0, 0, .3]).attach_to(base)
gm.gen_dashstick(np.array([0, pos_o[1], pos_o[2]]),
np.array([0, pos_o[1], 0]),
thickness=.001,
rgba=[0, 0, 0, .3],
lsolid=.005,
lspace=.005).attach_to(base)
# gm.gen_dashstick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base)
gm.gen_dashstick(np.array([pos_o[0], 0, pos_o[2]]),
np.array([0, 0, pos_o[2]]),
thickness=.001,
rgba=[0, 0, 0, .3],
lsolid=.005,
lspace=.005).attach_to(base)
# gm.gen_dashstick(pos_o, np.array([0, pos_o[1], pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base)
gm.gen_dashstick(np.array([0, pos_o[1], pos_o[2]]),
np.array([0, 0, pos_o[2]]),
thickness=.001,
rgba=[0, 0, 0, .3],
lsolid=.005,
lspace=.005).attach_to(base)
# #
gm.gen_sphere(pos=pos_o, radius=.005, rgba=[0, 0, 0, 1]).attach_to(base)
# gm.gen_dashstick(np.zeros(3), pos_o, thickness=.003, rgba=[.3, .3, .3, 1], lsolid=.01, lspace=.01).attach_to(base)
gm.gen_stick(np.zeros(3), pos_o, thickness=.003, rgba=[.3, .3, .3,
1]).attach_to(base)
base.run()
def showplanedfacetvertices(self, num):
color = (random.random(), random.random(), random.random(), 1)
for vertice in self.hpairsvertices[num]:
for i in vertice:
gm.gen_sphere(pos=(i[0], i[1], i[2]), radius=0.005,
rgba=color).attach_to(base)
def genSphere(pos, radius=0.02, rgba=None):
if rgba is None:
rgba = [1, 0, 0, 1]
gm.gen_sphere(pos=pos, radius=radius, rgba=rgba).attach_to(base)
# robot_s.fk(component_name="arm", jnt_values=armjnts)
# is_robot_collided = robot_s.is_collided(obstacle_list=obstacle_list)
# # display only if armjnts is not None and rbt_s is not collided
# if is_robot_collided is False and counter is 0:
# dist_pos.append(jnt_values)
# counter += 1
# continue
# print(dist_pos)
# gpa.write_pickle_file('box', dist_pos, './', 'dist_pos.pickle')
dist_pos = gpa.load_pickle_file('box', './', 'dist_pos.pickle')
for i in dist_pos:
pos = i.copy()
pos[2] = 0
gm.gen_sphere(pos=pos).attach_to(base)
## ppp (tool ppp)
object_box = tool.copy()
target_obj = object_box4.copy()
hnd_name = "hnd"
ppp_s = ppp.PickPlacePlanner(robot_s)
start_conf = robot_s.get_jnt_values(component_name="arm")
goal_homomat_list = [tool_initial_homomat, tool_final_homomat]
for dict_goal_pos in dist_pos:
print("move to the dist pos", dict_goal_pos)
robot_s.fk(component_name="agv", jnt_values=dict_goal_pos)
conf_list1, jawwidth_list1, objpose_list1 = \
ppp_s.gen_pick_and_place_motion(hnd_name=hnd_name,
objcm=tool,
grasp_info_list=grasp_info_list_tool,
def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter,
task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
while True:
pkx.device_get_capture()
color_image_handle = pkx.capture_get_color_image()
depth_image_handle = pkx.capture_get_depth_image()
if color_image_handle and depth_image_handle:
break
point_cloud = pkx.transform_depth_image_to_point_cloud(depth_image_handle)
point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud)
ball = []
for id, point_cloud_sub in enumerate(point_cloud):
if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[
1] < .3 and 0.5 < point_cloud_sub[2] < 2.5:
ball.append(point_cloud_sub)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
if len(ball) > 20:
center = np.mean(np.array(ball), axis=0)
ball_center_list.append([center, counter[0]])
ball = gm.gen_sphere(center, radius=.1)
ball.attach_to(base)
ball_list.append(ball)
if len(ball_center_list) > 2:
x = []
y = []
z = []
t = []
for cen, f_id in ball_center_list:
x.append(cen[0])
y.append(cen[1])
z.append(cen[2])
t.append(f_id)
para_x = np.polyfit(t, x, 1)
para_y = np.polyfit(t, y, 1)
para_z = np.polyfit(t, z, 2)
f_x = np.poly1d(para_x)
f_y = np.poly1d(para_y)
f_z = np.poly1d(para_z)
orbit = []
for t in np.linspace(ball_center_list[0][1],
ball_center_list[0][1] + 15, 100):
point = np.array([f_x(t), f_y(t), f_z(t)])
orbit.append(point)
if abs(point[0]) < .7 and abs(
point[1]) < .7 and abs(point[2] - 1.1) < .3:
# last_point = orbit[-1]
jnt_values = rbts.ik(tgt_pos=point,
tgt_rotmat=np.array([[-1, 0, 0],
[0, -1, 0],
[0, 0, 1]]))
if jnt_values is None:
continue
rbts.fk(component_name="arm", jnt_values=jnt_values)
rbts.gen_meshmodel().attach_to(base)
rbtx.arm_move_jspace_path(
[rbtx.get_jnt_values(), jnt_values],
max_jntspeed=math.pi * 1.3)
break
for id in range(len(orbit)):
if id > 0:
tmp_stick = gm.gen_stick(spos=orbit[id - 1],
epos=orbit[id],
thickness=.01,
type="round")
tmp_stick.attach_to(base)
para_list.append(tmp_stick)
return task.done
pkx.image_release(color_image_handle)
pkx.image_release(depth_image_handle)
pkx.capture_release()
counter[0] += 1
return task.cont
homomat[:3, :3] = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2)
homomat[:3, 3] = np.array([0.02, 0.02, 0])
objcm1.set_homomat(homomat)
objcm1.set_rgba([1, 1, .3, .2])
objcm2 = objcm1.copy()
objcm2.set_pos(objcm1.get_pos() + np.array([.05, .02, .0]))
objcm1.change_cdmesh_type('convex_hull')
objcm2.change_cdmesh_type('obb')
iscollided, contact_points = is_collided(objcm1, objcm2)
objcm1.show_cdmesh()
objcm2.show_cdmesh()
objcm1.attach_to(base)
objcm2.attach_to(base)
print(iscollided)
for ctpt in contact_points:
gm.gen_sphere(ctpt, radius=.001).attach_to(base)
pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0])
# pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -1.0])
pto = np.array([0, 0, 0]) + np.array([0.02, 0.02, 0.02])
# pfrom = np.array([0, 0, 0]) + np.array([0.0, 0.0, 1.0])
# pto = np.array([0, 0, 0]) + np.array([0.0, 0.0, -1.0])
# hit_point, hit_normal = rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm1)
hit_points, hit_normals = rayhit_all(pfrom=pfrom, pto=pto, objcm=objcm1)
# objcm.attach_to(base)
# objcm.show_cdmesh(type='box')
# objcm.show_cdmesh(type='convex_hull')
# for hitpos, hitnormal in zip([hit_point], [hit_normal]):
for hitpos, hitnormal in zip(hit_points, hit_normals):
gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base)
gm.gen_arrow(hitpos, epos=hitpos+hitnormal*.03, thickness=.002, rgba=np.array([0, 1, 1, 1])).attach_to(base)
gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base)
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)
# cv2.waitKey(0)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
for pcd_pnt in pcd_pnt_list:
marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1)
marker_center.attach_to(base)
base.run()
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
base.run()
print("pcd_result(bw glass1 and star1):{}".format(
pcd_result1)) # 衝突の結果を出力します
# star1, star2
pcd_result2 = star1.is_pcdwith(star2)
print("pcd_result(bw star1 and star2):{}".format(
pcd_result2)) # 衝突の結果を出力します
# glass2, star2
pcd_result3 = glass2.is_pcdwith(star2)
print("pcd_result(bw glass2 and star2):{}".format(
pcd_result3)) # 衝突の結果を出力します
# mesh間の衝突を検出します
# glass1, star1
mcd_result1, cd_points1 = glass1.is_mcdwith(star1, toggle_contacts=True)
for pnt in cd_points1:
gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base)
print("mcd_result(bw glass1 and star1):{}".format(
mcd_result1)) # 衝突の結果を出力します
# star1, star2
mcd_result2, cd_points2 = star1.is_mcdwith(star2, toggle_contacts=True)
for pnt in cd_points2:
gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base)
print("mcd_result(bw star1 and star2):{}".format(
mcd_result2)) # 衝突の結果を出力します
# glass2, star2
mcd_result3, cd_points3 = glass2.is_mcdwith(star2, toggle_contacts=True)
for pnt in cd_points3:
gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base)
print("mcd_result(bw glass2 and star2):{}".format(
mcd_result3)) # 衝突の結果を出力します
rgba=[0, 0, 0, 1],
thickness=.002,
type='round').attach_to(base)
epos = (line_segs[0][1] - line_segs[0][0]) * .7 + line_segs[0][0]
gm.gen_arrow(spos=line_segs[0][0], epos=epos, thickness=0.004).attach_to(base)
spt = homomat[:3, 3]
# gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base)
# base.run()
gm.gen_dasharrow(spt, spt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
cpt, cnrml = bowl_model.ray_hit(spt,
spt + pn_direction * 10000,
option='closest')
gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7],
thickness=0.003).attach_to(base)
gm.gen_sphere(pos=cpt, radius=.005).attach_to(base)
gm.gen_dasharrow(cpt, cpt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
gm.gen_dasharrow(cpt, cpt + cnrml * .07, thickness=.004).attach_to(base) # p0
angle = rm.angle_between_vectors(-pn_direction, cnrml)
vec = np.cross(-pn_direction, cnrml)
rotmat = rm.rotmat_from_axangle(vec, angle)
new_plane_homomat = np.eye(4)
new_plane_homomat[:3, :3] = rotmat.dot(homomat[:3, :3])
new_plane_homomat[:3, 3] = cpt
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=new_plane_homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
new_line_segs = [
rbt_s.gen_meshmodel().attach_to(base)
planner = rrtc.RRTConnect(rbt_s)
ee_s = cbtg.CobottaPipette()
id_x = 7
id_y = 11
tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
z_offset = np.array([0, 0, .02])
base.change_campos_and_lookat_pos(cam_pos=[.4, .0, .15],
lookat_pos=tip_pos + z_offset)
spiral_points = rm.gen_3d_equilateral_verts(pos=tip_pos + z_offset,
rotmat=tip_rotmat)
print(spiral_points)
pre_point = None
for point in spiral_points:
gm.gen_sphere(point, radius=.00016).attach_to(base)
if pre_point is not None:
gm.gen_stick(pre_point, point, thickness=.00012).attach_to(base)
pre_point = point
goal_joint_values_attachment = utils.search_reachable_configuration(
rbt_s=rbt_s,
ee_s=ee_s,
component_name=component_name,
tgt_pos=spiral_points[0],
cone_axis=-tip_rotmat[:3, 2],
rotation_interval=np.radians(15),
obstacle_list=[frame_bottom],
toggle_debug=False)
if goal_joint_values_attachment is not None:
rbt_s.fk(component_name=component_name,
def gen_circular_motion(self,
component_name,
circle_center_pos,
circle_ax,
start_tcp_rotmat,
end_tcp_rotmat,
radius=.02,
obstacle_list=[],
granularity=0.03,
seed_jnt_values=None,
toggle_tcp_list=False,
toggle_debug=False):
"""
:param component_name:
:param start_tcp_pos:
:param start_tcp_rotmat:
:param goal_tcp_pos:
:param goal_tcp_rotmat:
:param goal_info:
:param obstacle_list:
:param granularity:
:return:
author: weiwei
date: 20210501
"""
jnt_values_bk = self.robot_s.get_jnt_values(component_name)
pos_list, rotmat_list = rm.interplate_pos_rotmat_around_circle(
circle_center_pos,
circle_ax,
radius,
start_tcp_rotmat,
end_tcp_rotmat,
granularity=granularity)
# for (pos, rotmat) in zip(pos_list, rotmat_list):
# gm.gen_frame(pos, rotmat).attach_to(base)
# base.run()
jnt_values_list = []
if seed_jnt_values is None:
seed_jnt_values = jnt_values_bk
for (pos, rotmat) in zip(pos_list, rotmat_list):
jnt_values = self.robot_s.ik(component_name,
pos,
rotmat,
seed_jnt_values=seed_jnt_values)
if jnt_values is None:
print("IK not solvable in gen_circular_motion!")
self.robot_s.fk(component_name, jnt_values_bk)
return []
else:
self.robot_s.fk(component_name, jnt_values)
cd_result, ct_points = self.robot_s.is_collided(
obstacle_list, toggle_contact_points=True)
if cd_result:
if toggle_debug:
for ct_pnt in ct_points:
gm.gen_sphere(ct_pnt).attach_to(base)
print("Intermediate pose collided in gen_linear_motion!")
self.robot_s.fk(component_name, jnt_values_bk)
return []
jnt_values_list.append(jnt_values)
seed_jnt_values = jnt_values
self.robot_s.fk(component_name, jnt_values_bk)
if toggle_tcp_list:
return jnt_values_list, [[pos_list[i], rotmat_list[i]]
for i in range(len(pos_list))]
else:
return jnt_values_list
def __showSptPnt(self, SptPnt, color):
gm.gen_sphere(pos=Vec3(SptPnt[0], SptPnt[1], SptPnt[2]),
radius=20,
rgba=color).attach_to(base)
