def trans_data_pcv(data, toggledebug=False, random_rot=True):
pcv, pcaxmat = rm.compute_pca(data)
inx = sorted(range(len(pcv)), key=lambda k: pcv[k])
x_v = pcaxmat[:, inx[2]]
y_v = pcaxmat[:, inx[1]]
z_v = pcaxmat[:, inx[0]]
pcaxmat = np.asarray([y_v, -x_v, -z_v]).T
if random_rot:
pcaxmat = np.dot(rm.rotmat_from_axangle([1, 0, 0], math.radians(5)),
pcaxmat)
pcaxmat = np.dot(rm.rotmat_from_axangle([0, 1, 0], math.radians(5)),
pcaxmat)
pcaxmat = np.dot(rm.rotmat_from_axangle([0, 0, 1], math.radians(5)),
pcaxmat)
data_tr = np.dot(pcaxmat.T, data.T).T
if toggledebug:
center = get_pcd_center(data)
print('center:', center)
scale = 2
ax.arrow3D(center[0],
center[1],
center[2],
scale * x_v[0],
scale * x_v[1],
scale * x_v[2],
mutation_scale=10,
arrowstyle='->',
color='r')
ax.arrow3D(center[0],
center[1],
center[2],
scale * y_v[0],
scale * y_v[1],
scale * y_v[2],
mutation_scale=10,
arrowstyle='->',
color='g')
ax.arrow3D(center[0],
center[1],
center[2],
scale * z_v[0],
scale * z_v[1],
scale * z_v[2],
mutation_scale=10,
arrowstyle='->',
color='b')
ax.scatter(data[:, 0], data[:, 1], data[:, 2], c='r', s=5, alpha=.5)
ax.scatter(data_tr[:, 0],
data_tr[:, 1],
data_tr[:, 2],
c='g',
s=5,
alpha=.5)
center = np.mean(data_tr, axis=0)
data_tr = data_tr - center
transmat = np.eye(4)
transmat[:3, :3] = pcaxmat
transmat[:3, 3] = np.mean(data, axis=0)
return data_tr, transmat
def computehomomat(RotMatnozero,height,error):
roterror_x = rm.rotmat_from_axangle(axis=(1, 0, 0), angle=np.radians(error[3]))
roterror_y = rm.rotmat_from_axangle(axis=(0, 1, 0), angle=np.radians(error[4]))
roterror_z = rm.rotmat_from_axangle(axis=(0, 0, 1), angle=np.radians(error[5]))
roterror = rm.homomat_from_posrot(rot = np.dot(roterror_x, np.dot(roterror_y, roterror_z)))
rotating=rm.rotmat_from_axangle(axis=(0,0,1),angle=np.radians(180))
moving=np.array([0.600+error[0],0+error[1],height+error[2]])
objT=rm.homomat_from_posrot(moving, rotating)
objhomomat=np.dot(objT,np.dot(da.pdmat4_to_npmat4(RotMatnozero),roterror))
return objhomomat
def computehomomatforcom():
rotating = rm.rotmat_from_axangle(axis=(0, 0, 1), angle=np.radians(180))
moving = np.array([.600, 0, .780])
objT = rm.homomat_from_posrot(moving, rotating)
# objT=da.pdmat4_to_npmat4((p3du.npToMat4(rotating, moving))
# objhomomat=np.dot(objT,p3du.mat4ToNp(RotMatnozero))
return objT
def _update_jnt_fk(self, jnt_name):
"""
update fk tree recursively
author: weiwei
date: 20201204osaka
"""
p_jnt_name = self.jnt_collection[jnt_name].p_name
cur_jnt = self.jnt_collection[jnt_name]
# update gl_pos0 and gl_rotmat0
if p_jnt_name is None:
cur_jnt.gl_pos0 = cur_jnt.loc_pos
cur_jnt.gl_rotmat0 = cur_jnt.loc_rotmat
else:
p_jnt = self.jnt_collection[p_jnt_name]
curjnt_loc_pos = np.dot(p_jnt.gl_rotmatq, cur_jnt.loc_pos)
cur_jnt.gl_pos0 = p_jnt.gl_posq + curjnt_loc_pos
cur_jnt.gl_rotmat0 = np.dot(p_jnt.gl_rotmatq, cur_jnt.loc_rotmat)
cur_jnt.gl_motionax = np.dot(cur_jnt.gl_rotmat0,
cur_jnt.loc_motionax)
# update gl_pos_q and gl_rotmat_q
if cur_jnt.type == "dummy":
cur_jnt.gl_posq = cur_jnt.gl_pos0
cur_jnt.gl_rotmatq = cur_jnt.gl_rotmat0
elif cur_jnt.type == "revolute":
cur_jnt.gl_posq = cur_jnt.gl_pos0
curjnt_loc_rotmat = rm.rotmat_from_axangle(cur_jnt.loc_motionax,
cur_jnt.motion_val)
cur_jnt.gl_rotmatq = np.dot(cur_jnt.gl_rotmat0, curjnt_loc_rotmat)
elif cur_jnt.type == "prismatic":
cur_jnt.gl_posq = cur_jnt.gl_pos0 + cur_jnt.motion_val * cur_jnt.loc_motionax
cur_jnt.gl_rotmatq = cur_jnt.gl_rotmat0
else:
raise ValueError("The given joint type is not available!")
for each_jnt_name in cur_jnt.chd_name_list:
self._update_jnt_fk(each_jnt_name)
def rot_top(self, angle):
rot = rm.rotmat_from_axangle((0, 0, 1), angle / 180 * np.pi)
mat = da.npv3mat3_to_pdmat4((0, 0, 0), rot)
for i, cube in enumerate(self.node_list):
if round(cube.getPos()[2], 2) == 0.02:
print(i)
self.node_list[i].setMat(
da.npmat4_to_pdmat4(
np.dot(da.pdmat4_to_npmat4(mat),
da.pdmat4_to_npmat4(
self.node_list[i].getMat()))))
def getCoordinate(self):
coordinata = []
doubleholdpair = []
for pair in self.hpairs:
normal_0 = -self.facetnormals[pair[0]]
normal_1 = -self.facetnormals[pair[1]]
# normal_1 = normal_1, rm.rotmat_from_axangle()
normal_2a = np.cross(normal_0, normal_1)
normal_2b = np.cross(normal_1, normal_0)
normal_0 = np.dot( rm.rotmat_from_axangle(normal_2a, np.radians(+15)),normal_0)
normal_1 = np.dot(rm.rotmat_from_axangle(normal_2a, np.radians(-15)), normal_1)
coordinate0 = np.array([normal_0, normal_1, normal_2a])
coordinate1 = np.array([normal_1, normal_0, normal_2b])
coordinata.append(coordinate0.T)
coordinata.append(coordinate1.T)
doubleholdpair.append(pair)
doubleholdpair.append(pair)
self.coordinate = coordinata
self.doubleholdpair = doubleholdpair
self.gettwoend()
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 _find_tcp_in_sensor(self, component_name, action_pos, action_rotmat, sensor_marker_handler):
"""
find the robot_s tcp's pos and rotmat in the sensor coordinate system
:param component_name:
:param action_center_pos, action_rotmat:
:param marker_callback:
:return: [estiamted tcp center in sensor, radius of the sphere formed by markers]
author: weiwei
date: 20210408
"""
def _fit_sphere(p, coords):
x0, y0, z0, radius = p
x, y, z = coords.T
return np.sqrt((x - x0) ** 2 + (y - y0) ** 2 + (z - z0) ** 2)
_err_fit_sphere = lambda p, x: _fit_sphere(p, x) - p[3]
marker_pos_in_sensor_list = []
rot_range_x = [np.array([1, 0, 0]), [-30, -15, 0, 15, 30]]
rot_range_y = [np.array([0, 1, 0]), [-30, -15, 15, 30]]
rot_range_z = [np.array([0, 0, 1]), [-90, -60, -30, 30, 60]]
range_axes = [rot_range_x, rot_range_y, rot_range_z]
last_jnt_values = self.robot_x.lft_arm_hnd.get_jnt_values()
jnt_values_bk = self.robot_s.get_jnt_values(component_name)
for axisid in range(3):
axis = range_axes[axisid][0]
for angle in range_axes[axisid][1]:
goal_pos = action_pos
goal_rotmat = np.dot(rm.rotmat_from_axangle(axis, angle), action_rotmat)
jnt_values = self.robot_s.ik(component_name=component_name,
tgt_pos=goal_pos,
tgt_rotmat=goal_rotmat,
seed_jnt_values=last_jnt_values)
self.robot_s.fk(component_name=component_name, jnt_values=jnt_values)
if jnt_values is not None and not self.robot_s.is_collided():
last_jnt_values = jnt_values
self.robot_x.move_jnts(component_name, jnt_values)
marker_pos_in_sensor = sensor_marker_handler.get_marker_center()
if marker_pos_in_sensor is not None:
marker_pos_in_sensor_list.append(marker_pos_in_sensor)
self.robot_s.fk(component_name=component_name, jnt_values=jnt_values_bk)
if len(marker_pos_in_sensor_list) < 3:
return [None, None]
center_in_camera_coords_array = np.asarray(marker_pos_in_sensor_list)
# try:
initial_guess = np.ones(4)*.001
initial_guess[:3] = np.mean(center_in_camera_coords_array, axis=0)
final_estimate, flag = sopt.leastsq(_err_fit_sphere, initial_guess, args=(center_in_camera_coords_array,))
if len(final_estimate) == 0:
return [None, None]
return np.array(final_estimate[:3]), final_estimate[3]
def genHexahedralVectors(coefficient,polygon = 6, normal = np.array([0,0,1]), Fdistributed=1):
'''generate vectors for simulating the friction cone'''
initalMatrix = np.zeros([polygon,3])
# initVector =pg.rm.unit_vector(np.array([0,coefficient,1]))
initVector = Fdistributed*np.array([0, coefficient, 1])
# tfMatrix = pg.trigeom.align_vectors(vector_start=np.array([0,0,1]),vector_end=-normal)
tfMatrix = trigeom.align_vectors(vector_start=np.array([0, 0, 1]), vector_end=normal)
initalMatrix[0,:] = rm.homomat_transform_points(tfMatrix,initVector)
angle = 360.0/ polygon
for i in range(1,polygon):
rotMat = rm.rotmat_from_axangle([0,0,1],angle*i)
initalMatrix[i,:] = rm.homomat_transform_points(tfMatrix,np.dot(rotMat,initVector))
return initalMatrix
def _update_fk(self):
"""
Update the kinematics
Note that this function should not be called explicitly
It is called automatically by functions like movexxx
:return: updated links and joints
author: weiwei
date: 20161202, 20201009osaka
"""
id = 0
while id != -1:
# update joint values
pjid = self.jnts[id]['parent']
if pjid == -1:
self.jnts[id]['gl_pos0'] = self.pos
self.jnts[id]['gl_rotmat0'] = self.rotmat
else:
self.jnts[id]['gl_pos0'] = self.jnts[pjid]['gl_posq'] + np.dot(
self.jnts[pjid]['gl_rotmatq'], self.jnts[id]['loc_pos'])
self.jnts[id]['gl_rotmat0'] = np.dot(
self.jnts[pjid]['gl_rotmatq'], self.jnts[id]['loc_rotmat'])
self.jnts[id]['gl_motionax'] = np.dot(
self.jnts[id]['gl_rotmat0'], self.jnts[id]['loc_motionax'])
if self.jnts[id]['type'] == "end" or self.jnts[id][
'type'] == "fixed":
self.jnts[id]['gl_rotmatq'] = self.jnts[id]['gl_rotmat0']
self.jnts[id]['gl_posq'] = self.jnts[id]['gl_pos0']
elif self.jnts[id]['type'] == "revolute":
self.jnts[id]['gl_rotmatq'] = np.dot(
self.jnts[id]['gl_rotmat0'],
rm.rotmat_from_axangle(self.jnts[id]['loc_motionax'],
self.jnts[id]['motion_val']))
self.jnts[id]['gl_posq'] = self.jnts[id]['gl_pos0']
elif self.jnts[id]['type'] == "prismatic":
self.jnts[id]['gl_rotmatq'] = self.jnts[id]['gl_rotmat0']
tmp_translation = np.dot(
self.jnts[id]['gl_rotmatq'],
self.jnts[id]['loc_motionax'] *
self.jnts[id]['motion_val'])
self.jnts[id][
'gl_posq'] = self.jnts[id]['gl_pos0'] + tmp_translation
# update link values, child link id = id
if id < self.ndof + 1:
self.lnks[id]['gl_pos'] = np.dot(self.jnts[id]['gl_rotmatq'], self.lnks[id]['loc_pos']) + \
self.jnts[id]['gl_posq']
self.lnks[id]['gl_rotmat'] = np.dot(
self.jnts[id]['gl_rotmatq'], self.lnks[id]['loc_rotmat'])
# self.lnks[id]['cdprimit_cache'][0] = True
id = self.jnts[id]['child']
return self.lnks, self.jnts
import os
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta_ripps as cbtr
import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg
import modeling.collision_model as cm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import utils
if __name__ == '__main__':
base = wd.World(cam_pos=[.25, -1, .4], lookat_pos=[.25, 0, .3])
gm.gen_frame().attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
# rbt_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
rbt_s.jaw_to(jaw_width=0.03)
tgt_pos = np.array([.25, .0, .1])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], np.pi)
jnt_values = rbt_s.ik(component_name="arm",
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat)
print(jnt_values)
if jnt_values is not None:
rbt_s.fk(component_name="arm", jnt_values=jnt_values)
rbt_s.gen_meshmodel().attach_to(base)
base.run()
import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.end_effectors.gripper.robotiq85.robotiq85 as rtq85 import robot_sim.end_effectors.gripper.robotiqhe.robotiqhe as rtqhe base = wd.World(cam_pos=np.array([1,1,1])) pos0 = np.array([0,0.07,.3]) rotmat0 = rm.rotmat_from_axangle([1,0,0], np.pi/12) rotmat0 = rm.rotmat_from_axangle([0,1,0], np.pi/12) rotmat0 = rm.rotmat_from_axangle([0,0,1], np.pi/9) hnd0 = rtqhe.RobotiqHE() hnd0.grip_at_with_jcpose(gl_jaw_center_pos=pos0, gl_jaw_center_rotmat=rotmat0, jaw_width=.005) hnd0.gen_meshmodel().attach_to(base) pos1 = np.array([0,-0.07,.3]) rotmat1 = rm.rotmat_from_axangle([-1,0,0], np.pi/12) rotmat1 = rm.rotmat_from_axangle([0,-1,0], np.pi/12) rotmat1 = rm.rotmat_from_axangle([0,0,-1], np.pi/9) hnd1 = rtqhe.RobotiqHE() hnd1.grip_at_with_jcpose(gl_jaw_center_pos=pos1, gl_jaw_center_rotmat=rotmat1, jaw_width=.005) hnd1.gen_meshmodel().attach_to(base) base.run()
import numpy as np
import basis.robot_math as rm
import visualization.panda.world as wd
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtq_he
import modeling.geometric_model as gm
if __name__ == "__main__":
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
gm.gen_frame(length=.2).attach_to(base)
grpr = rtq_he.RobotiqHE(enable_cc=True)
grpr.jaw_to(.05)
grpr.gen_meshmodel(rgba=[.3,.3,.3,.3]).attach_to(base)
grpr.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base)
grpr.fix_to(pos=np.array([-.1, .2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05))
grpr.gen_meshmodel().attach_to(base)
grpr.show_cdmesh()
grpr.fix_to(pos=np.array([.1, -.2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05))
grpr.gen_meshmodel().attach_to(base)
grpr.show_cdprimit()
base.run()
table_plate = cm.gen_box(extent=[.405, .26, .003])
table_plate.set_pos([0.07 + 0.2025, .055, .0015])
table_plate.set_rgba([.87, .87, .87, 1])
table_plate.attach_to(base)
file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box, expand_radius=.007)
dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
dispose_box.set_pos(pos=np.array([.14, 0.07, .003]))
dispose_box.attach_to(base)
file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
tip_rack = utils.Rack96(file_tip_rack)
tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
tip_rack.set_pose(pos=np.array([.35, 0.0, .003]), rotmat=rm.rotmat_from_axangle([0, 0, 1], 0))
tip_rack.attach_to(base)
file_tip = os.path.join(this_dir, "objects", "tip.stl")
tip = cm.CollisionModel(file_tip)
tip.set_rgba([200 / 255, 180 / 255, 140 / 255, 1])
tip_cm_list = []
for id_x in range(8):
for id_y in range(12):
pos, rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
tip_new = tip.copy()
tip_new.set_pose(pos, rotmat)
# gm.gen_frame(pos=pos, rotmat=rotmat).attach_to(base)
tip_new.attach_to(base)
tip_cm_list.append(tip_new)
import pandas as pd
import neuro.ik.cobotta_fitting as cbf
import basis.robot_math as rm
import modeling.geometric_model as gm
import visualization.panda.world as world
import robot_sim.robots.cobotta.cobotta as cbt_s
if __name__ == '__main__':
base = world.World(cam_pos=np.array([1.5, 1, .7]))
gm.gen_frame().attach_to(base)
rbt_s = cbt_s.Cobotta()
rbt_s.fk(jnt_values=np.zeros(6))
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5,.5,.5,.3]).attach_to(base)
rbt_s.gen_stickmodel(toggle_tcpcs=True).attach_to(base)
tgt_pos = np.array([.25, .2, .2])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi * 3 / 3)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
contraint_pos = rbt_s.manipulator_dict['arm'].jnts[5]['gl_posq']
contraint_rotmat = rbt_s.manipulator_dict['arm'].jnts[5]['gl_rotmatq']
gm.gen_frame(pos=contraint_pos, rotmat=contraint_rotmat).attach_to(base)
# numerical ik
jnt_values = rbt_s.ik(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat)
rbt_s.fk(jnt_values=jnt_values)
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5,.5,.5,.3]).attach_to(base)
rbt_s.gen_stickmodel(toggle_tcpcs=True).attach_to(base)
contraint_pos = rbt_s.manipulator_dict['arm'].jnts[5]['gl_posq']
contraint_rotmat = rbt_s.manipulator_dict['arm'].jnts[5]['gl_rotmatq']
contraint_pos += rbt_s.manipulator_dict['arm'].jnts[6]['loc_pos'][1] * contraint_rotmat[:, 1]
gm.gen_frame(pos=contraint_pos, rotmat=contraint_rotmat).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 = [
[cpt, cpt + rotmat.dot(pt_direction) * .05],
[
cpt + rotmat.dot(pt_direction) * .05,
cpt + rotmat.dot(pt_direction) * .05 + rotmat.dot(tmp_direction) * .05
],
[
cpt + rotmat.dot(pt_direction) * .05 + rotmat.dot(tmp_direction) * .05,
return meshmodel
if __name__ == '__main__':
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[.5, .5, .5], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
grpr = YumiGripper(enable_cc=True)
grpr.fix_to(pos=np.array([0, .3, .2]),
rotmat=rm.rotmat_from_euler(math.pi / 3, math.pi / 3,
math.pi / 3))
grpr.jaw_to(.02)
print(grpr.get_jawwidth())
grpr.gen_stickmodel().attach_to(base)
grpr.gen_meshmodel(rgba=[0, .5, 0, .5]).attach_to(base)
# grpr.gen_stickmodel(togglejntscs=False).attach_to(base)
# grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_axangle([1, 0, 0], math.pi/3))
grpr.fix_to(pos=np.zeros(3), rotmat=np.eye(3))
grpr.gen_meshmodel().attach_to(base)
grpr2 = grpr.copy()
grpr2.fix_to(pos=np.array([.3, .3, .2]),
rotmat=rm.rotmat_from_axangle([0, 1, 0], .01))
model = grpr2.gen_meshmodel(rgba=[0.5, .5, 0, .5])
model.attach_to(base)
grpr2.show_cdprimit()
grpr2.show_cdmesh()
base.run()
table_plate = cm.gen_box(extent=[.405, .26, .003])
table_plate.set_pos([0.07 + 0.2025, .055, .0015])
table_plate.set_rgba([.87, .87, .87, 1])
# table_plate.attach_to(base)
file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box, expand_radius=.007)
dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
dispose_box.set_pos(pos=np.array([.14, 0.07, .003]))
# dispose_box.attach_to(base)
file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
tip_rack = utils.Rack96(file_tip_rack)
tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, .3])
tip_rack.set_pose(pos=np.array([.25, 0.0, .003]),
rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
tip_rack.attach_to(base)
file_tip = os.path.join(this_dir, "objects", "tip.stl")
tip = cm.CollisionModel(file_tip)
tip.set_rgba([200 / 255, 180 / 255, 140 / 255, .3])
tip_cm_list = []
for id_x in range(8):
for id_y in range(12):
pos, rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
tip_new = tip.copy()
tip_new.set_pose(pos, rotmat)
# gm.gen_frame(pos=pos, rotmat=rotmat).attach_to(base)
tip_new.attach_to(base)
tip_cm_list.append(tip_new)
object_fixture = object.copy()
object_fixture_pos = fixture_start_pos + np.array([0, 0, 0.030])
object_fixture_rotmat = np.eye(3)
object_fixture_homomat = rm.homomat_from_posrot(
object_fixture_pos,
object_fixture_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture.set_homomat(object_fixture_homomat)
# object_fixture.attach_to(base)
# object_goal = object_fixture
object_start = object.copy()
# object_start_pos = np.array([0.900, -.350, 0.800])
object_start_pos = np.array([0.950, -.350, 0.800])
object_start_rotmat = rm.rotmat_from_axangle(
(1, 0, 0),
np.radians(-90)).dot(rm.rotmat_from_axangle((0, 0, 1),
np.radians(180)))
object_start_homomat = rm.homomat_from_posrot(object_start_pos,
object_start_rotmat)
object_start.set_pos(object_start_pos)
object_start.set_rotmat(object_start_rotmat)
# object_start.attach_to(base)
grasp_info_list = gpa.load_pickle_file('test_long', './', 'rtqhe.pickle')
# show_ikfeasible_poses(object_fixture_homomat[:3,:3], object_fixture_pos)
# show_ikfeasible_poses(object_start_homomat[:3, :3], object_start_pos)
# base.run()
object_fixture_corner = object.copy()
object_fixture_corner_pos = fixture_start_pos
table_plate.set_rgba([.87, .87, .87, 1])
table_plate.attach_to(base)
file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box)
dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
dispose_box.set_pos(pos=np.array([.14, 0.07, .003]))
dispose_box.attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
component_name = "arm"
file_tip = os.path.join(this_dir, "objects", "tip.stl")
tip = cm.CollisionModel(file_tip)
tip.set_rgba([200 / 255, 180 / 255, 140 / 255, 1])
pos, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name)
tip.set_pose(pos, rm.rotmat_from_axangle(rotmat[:, 0], np.pi).dot(rotmat))
rbt_s.hold(hnd_name="hnd", objcm=tip)
ee_s = cbtg.CobottaPipette()
pos = dispose_box.get_pos() + np.array([0, 0.05, .02])
z_offset = np.array([0, 0.0, .03])
rotmat = rm.rotmat_from_axangle([1, 0, 0], -np.pi * 4 / 9).dot(
rm.rotmat_from_axangle([0, 1, 0], -np.pi))
utils.search_reachable_configuration(rbt_s=rbt_s,
ee_s=ee_s,
component_name=component_name,
tgt_pos=pos + z_offset,
cone_axis=rotmat[:3, 2],
cone_angle=np.pi / 18,
rotation_interval=np.radians(22.5),
obstacle_list=[frame_bottom],
jaw_center_gl_pos = self.rotmat.dot(self.jaw_center_pos)+self.pos
jaw_center_gl_rotmat = self.rotmat.dot(self.jaw_center_rotmat)
gm.gen_dashstick(spos=self.pos,
epos=jaw_center_gl_pos,
thickness=.0062,
rgba=[.5,0,1,1],
type="round").attach_to(meshmodel)
gm.gen_mycframe(pos=jaw_center_gl_pos, rotmat=jaw_center_gl_rotmat).attach_to(meshmodel)
return meshmodel
if __name__ == '__main__':
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[.5, .5, .5], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# for angle in np.linspace(0, .85, 8):
# grpr = Robotiq85()
# grpr.fk(angle)
# grpr.gen_meshmodel().attach_to(base)
grpr = CobottaGripper(enable_cc=True)
grpr.jaw_to(.013)
grpr.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
# grpr.gen_stickmodel(toggle_jntscs=False).attach_to(base)
grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05))
grpr.gen_meshmodel().attach_to(base)
grpr.show_cdmesh()
grpr.show_cdprimit()
base.run()
else:
return jnt_values_list
if __name__ == '__main__':
import time
import robot_sim.robots.yumi.yumi as ym
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[1.5, 0, 3], lookat_pos=[0, 0, .5])
gm.gen_frame().attach_to(base)
yumi_instance = ym.Yumi(enable_cc=True)
component_name = 'rgt_arm'
start_pos = np.array([.5, -.3, .3])
start_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_pos = np.array([.55, .3, .5])
goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
gm.gen_frame(pos=start_pos, rotmat=start_rotmat).attach_to(base)
gm.gen_frame(pos=goal_pos, rotmat=goal_rotmat).attach_to(base)
inik = IncrementalNIK(yumi_instance)
tic = time.time()
jnt_values_list = inik.gen_linear_motion(component_name,
start_tcp_pos=start_pos,
start_tcp_rotmat=start_rotmat,
goal_tcp_pos=goal_pos,
goal_tcp_rotmat=goal_rotmat)
toc = time.time()
print(toc - tic)
for jnt_values in jnt_values_list:
yumi_instance.fk(component_name, jnt_values)
base = wd.World(cam_pos=np.array([.6, .3, .8]), lookat_pos=np.zeros(3))
gm.gen_frame(length=.2, thickness=.01).attach_to(base) # グローバル座標系
# glassと星形のモデルのファイルを用いてCollisionModelを初期化します
# glass1, star1はmesh(convex_hull)の設定で作成します
glass1 = cm.CollisionModel(initor="objects/glass1.stl",
cdprimit_type="surface_balls",
cdmesh_type="convex_hull")
glass1.set_rgba([.9, .75, .35, 1]) # 黄色に変更
glass1.set_pos(np.array([0, -.06, 0]))
star1 = cm.CollisionModel(initor="objects/star2.stl",
cdprimit_type="surface_balls",
cdmesh_type="convex_hull")
star1.set_rgba([.9, .75, .35, 1]) # 黄色に変更
star1.set_pos(np.array([0, .01, .07]))
star1.set_rotmat(rm.rotmat_from_axangle(axis=[0, 0, 1],
angle=math.pi / 2.))
# glass1, star1をそれぞれコピーし，mesh(triangles)に変更してglass2, star2を作成します
glass2 = glass1.copy()
glass2.change_cdmesh_type(cdmesh_type="triangles")
glass2.set_pos(np.array([.02, .13, .025]))
glass2.set_rgba([.75, .35, .9, 1]) # 紫色に変更
star2 = star1.copy()
star2.change_cdmesh_type(cdmesh_type="triangles")
star2.set_pos(np.array([.01, .085, .1]))
star2.set_rgba([.75, .35, .9, 1]) # 紫色に変更
# primitive間の衝突を検出します
# glass1, star1
pcd_result1 = glass1.is_pcdwith(star1)
print("pcd_result(bw glass1 and star1):{}".format(
import basis
base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
nxt_instance = Nextage(enable_cc=True)
nxt_meshmodel = nxt_instance.gen_meshmodel(toggle_tcpcs=True)
nxt_meshmodel.attach_to(base)
# nxt_instance.show_cdprimit()
base.run()
# tgt_pos = np.array([.4, 0, .2])
# tgt_rotmat = rm.rotmat_from_euler(0, math.pi * 2 / 3, -math.pi / 4)
# ik test
component_name = 'lft_arm_waist'
tgt_pos = np.array([-.3, .45, .55])
tgt_rotmat = rm.rotmat_from_axangle([0, 0, 1], -math.pi / 2)
# tgt_rotmat = np.eye(3)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
tic = time.time()
jnt_values = nxt_instance.ik(component_name,
tgt_pos,
tgt_rotmat,
toggle_debug=True)
toc = time.time()
print(toc - tic)
nxt_instance.fk(component_name, jnt_values)
nxt_meshmodel = nxt_instance.gen_meshmodel()
nxt_meshmodel.attach_to(base)
nxt_instance.gen_stickmodel().attach_to(base)
# tic = time.time()
# result = nxt_instance.is_collided()
base = wd.World(cam_pos=[4, -1, 2], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
object_box = cm.gen_box(extent=[.15,.15,.15])
object_box.set_pos(np.array([.4, .3, .4]))
object_box.set_rgba([.5, .7, .3, 1])
object_box.attach_to(base)
# robot_s
component_name = 'lft_arm'
robot_s = nxt.Nextage()
# start_pos = np.array([.4, 0, .2])
# start_rotmat = rm.rotmat_from_euler(0, math.pi * 2 / 3, -math.pi / 4)
start_pos = np.array([.4, .1, .1])
start_rotmat = rm.rotmat_from_axangle([0,1,0], -math.pi/2)
start_conf = robot_s.ik(component_name, start_pos, start_rotmat)
# goal_pos = np.array([.3, .5, .7])
# goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi)
goal_pos = np.array([.3, .5, .6])
goal_rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi/2).dot(rm.rotmat_from_axangle([0, 1, 0], math.pi))
goal_conf = robot_s.ik(component_name, goal_pos, goal_rotmat)
print(start_conf, goal_conf)
rrtc_planner = rrtc.RRTConnect(robot_s)
path = rrtc_planner.plan(component_name=component_name,
start_conf=start_conf,
goal_conf=goal_conf,
obstacle_list=[object_box],
ext_dist=.1,
# print(iscollided)
# for ct_pnt in contact_points:
# gm.gen_sphere(ct_pnt, 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])
# # hitpos, hitnrml = rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm2)
# # 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()
wd.World(cam_pos=[.3, -.3, .3], lookat_pos=[0, 0, 0])
objpath = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl')
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)
import visualization.panda.world as wd
import basis.robot_math as rm
import math
base = wd.World(cam_pos=np.array([.7, -.7, 1]),
lookat_pos=np.array([0.3, 0, 0]))
gm.gen_frame().attach_to(base)
sec1_spos = np.array([0, 0, 0])
sec_len = np.array([.2, 0, 0])
sec1_epos = sec1_spos + sec_len
sec2_spos = sec1_epos
angle = math.pi / 5.7
sec2_rotaxis = np.array([0, 0, 1])
sec2_rotmat = rm.rotmat_from_axangle(sec2_rotaxis, angle)
sec2_epos = sec2_spos + sec2_rotmat.dot(sec_len)
rgba = [1, .4, 0, .3]
gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_frame(pos=sec2_spos, alpha=.2).attach_to(base)
# gm.gen_stick(spos=sec2_spos, epos=sec2_spos+sec_len, rgba=rgba, thickness=.015).attach_to(base)
# gm.gen_frame(pos=sec2_spos, rotmat=sec2_rotmat, alpha=.2).attach_to(base)
gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba,
thickness=.015).attach_to(base)
# gm.gen_circarrow(axis=sec2_rotaxis, center=sec2_rotaxis / 13+sec2_spos, starting_vector=np.array([-0,-1,0]),radius=.025, portion=.5, thickness=.003, rgba=[1,.4,0,1]).attach_to(base)
#
sec2_rotaxis2 = np.array([1, 0, 0])
sec2_rotmat2 = rm.rotmat_from_axangle(sec2_rotaxis2, angle)
sec2_epos = sec2_spos + sec2_rotmat2.dot(sec2_epos - sec2_spos)
# # sec2_rotmat2 = rm.rotmat_from_axangle([1,0,0], math.pi/6)
toggle_tcpcs=False,
toggle_jntscs=toggle_jntscs,
rgba=rgba).attach_to(meshmodel)
return meshmodel
if __name__ == '__main__':
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[.5, .5, .5], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
grpr = YumiGripper(enable_cc=True)
grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_euler(math.pi/3, math.pi/3, math.pi/3))
grpr.jaw_to(.02)
print(grpr.get_jawwidth())
grpr.gen_stickmodel().attach_to(base)
grpr.gen_meshmodel(rgba=[0, .5, 0, .5]).attach_to(base)
# grpr.gen_stickmodel(togglejntscs=False).attach_to(base)
# grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_axangle([1, 0, 0], math.pi/3))
grpr.fix_to(pos=np.zeros(3), rotmat=np.eye(3))
grpr.gen_meshmodel().attach_to(base)
grpr2 = grpr.copy()
grpr2.fix_to(pos=np.array([.3,.3,.2]), rotmat=rm.rotmat_from_axangle([0,1,0],.01))
model = grpr2.gen_meshmodel(rgba=[0.5, .5, 0, .5])
model.attach_to(base)
grpr2.show_cdprimit()
grpr2.show_cdmesh()
base.run()
this_dir, this_filename = os.path.split(__file__)
file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl")
frame_bottom = cm.CollisionModel(file_frame)
frame_bottom.set_rgba([.55, .55, .55, 1])
frame_bottom.attach_to(base)
table_plate = cm.gen_box(extent=[.405, .26, .003])
table_plate.set_pos([0.07 + 0.2025, .055, .0015])
table_plate.set_rgba([.87, .87, .87, 1])
table_plate.attach_to(base)
file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
tip_rack = utils.Rack96(file_tip_rack)
tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
tip_rack.set_pose(pos=np.array([.25, 0.0, .003]),
rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
tip_rack.attach_to(base)
file_tip = os.path.join(this_dir, "objects", "tip.stl")
tip = cm.CollisionModel(file_tip)
tip.set_rgba([200 / 255, 180 / 255, 140 / 255, 1])
for id_x in range(8):
for id_y in range(12):
pos, rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
tip_new = tip.copy()
tip_new.set_pose(pos, rotmat)
tip_new.attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
ee_s = cbtg.CobottaPipette()
import os
import math
import time
import numpy as np
import basis
import basis.robot_math as rm
import visualization.panda.world as wd
base = wd.World(cam_pos=[.3, .3, .3],
lookat_pos=[0, 0, 0],
toggle_debug=True)
objpath = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl')
bunnycm = CollisionModel(objpath, cdprimit_type='polygons')
bunnycm.set_rgba([0.7, 0.7, 0.0, .2])
bunnycm.show_localframe()
rotmat = rm.rotmat_from_axangle([1, 0, 0], math.pi / 2.0)
bunnycm.set_rotmat(rotmat)
bunnycm.show_cdprimit()
bunnycm1 = CollisionModel(objpath, cdprimit_type="cylinder")
bunnycm1.set_rgba([0.7, 0, 0.7, 1.0])
rotmat = rm.rotmat_from_euler(0, 0, math.radians(15))
bunnycm1.set_pos(np.array([0, .01, 0]))
bunnycm1.set_rotmat(rotmat)
bunnycm2 = bunnycm1.copy()
bunnycm2.change_cdprimitive_type(cdprimitive_type='surface_balls')
bunnycm2.set_rgba([0, 0.7, 0.7, 1.0])
rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi / 4.0)
bunnycm2.set_pos(np.array([0, .2, 0]))
bunnycm2.set_rotmat(rotmat)
