def _init_jlchain(self):
"""
init joints and links chains
there are two lists of dictionaries where the first one is joints, the second one is links
links: a list of dictionaries with each dictionary holding the properties of a link
joints: a list of dictionaries with each dictionary holding the properties of a joint
njoints is assumed to be equal to nlinks+1
joint i connects link i-1 and link i
:return:
author: weiwei
date: 20161202tsukuba, 20190328toyonaka, 20200330toyonaka
"""
lnks = [dict() for i in range(self.ndof + 1)]
jnts = [dict() for i in range(self.ndof + 2)]
for id in range(self.ndof + 1):
lnks[id]['name'] = 'link0'
lnks[id]['loc_pos'] = np.array([0, 0, 0])
lnks[id]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0)
lnks[id]['com'] = np.zeros(3)
lnks[id]['inertia'] = np.eye(3)
lnks[id][
'mass'] = 0 # the visual adjustment is ignored for simplisity
lnks[id]['meshfile'] = None
lnks[id]['collisionmodel'] = None
lnks[id][
'cdprimit_childid'] = -1 # id of the CollisionChecker.np.Child
lnks[id]['scale'] = [1, 1, 1] # 3 list
lnks[id]['rgba'] = [.7, .7, .7, 1] # 4 list
for id in range(self.ndof + 2):
jnts[id]['type'] = 'revolute'
jnts[id]['parent'] = id - 1
jnts[id]['child'] = id + 1
jnts[id]['loc_pos'] = np.array([0, .1, 0]) if id > 0 else np.array(
[0, 0, 0])
jnts[id]['loc_rotmat'] = np.eye(3)
jnts[id]['loc_motionax'] = np.array(
[0, 0, 1]) # rot ax for rev joint, linear ax for pris joint
jnts[id]['gl_pos0'] = jnts[id][
'loc_pos'] # to be updated by self._update_fk
jnts[id]['gl_rotmat0'] = jnts[id][
'loc_rotmat'] # to be updated by self._update_fk
jnts[id]['gl_motionax'] = jnts[id][
'loc_motionax'] # to be updated by self._update_fk
jnts[id]['gl_posq'] = jnts[id][
'gl_pos0'] # to be updated by self._update_fk
jnts[id]['gl_rotmatq'] = jnts[id][
'gl_rotmat0'] # to be updated by self._update_fk
jnts[id]['motion_rng'] = [-math.pi, math.pi] # min, max
jnts[id]['motion_val'] = 0
jnts[0]['gl_pos0'] = self.pos # This is not necessary, for easy read
jnts[0]['gl_rotmat0'] = self.rotmat
jnts[0]['type'] = 'end'
jnts[self.ndof + 1]['loc_pos'] = np.array([0, 0, 0])
jnts[self.ndof + 1]['child'] = -1
jnts[self.ndof + 1]['type'] = 'end'
return lnks, jnts
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
cdmesh_type='convex_hull',
name='yumi_gripper',
enable_cc=True):
super().__init__(pos=pos,
rotmat=rotmat,
cdmesh_type=cdmesh_type,
name=name)
this_dir, this_filename = os.path.split(__file__)
cpl_end_pos = self.coupling.jnts[-1]['gl_posq']
cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq']
# - lft
self.lft = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(1),
name='base_lft_finger')
self.lft.jnts[1]['loc_pos'] = np.array([0, 0.0065, 0.0837])
self.lft.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi)
self.lft.jnts[1]['type'] = 'prismatic'
self.lft.jnts[1]['motion_rng'] = [.0, .025]
self.lft.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.lft.lnks[0]['name'] = "base"
self.lft.lnks[0]['loc_pos'] = np.zeros(3)
self.lft.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes",
"base.stl")
self.lft.lnks[0]['rgba'] = [.5, .5, .5, 1]
self.lft.lnks[1]['name'] = "finger1"
self.lft.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes",
"finger.stl")
self.lft.lnks[1]['rgba'] = [.2, .2, .2, 1]
# - rgt
self.rgt = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(1),
name='rgt_finger')
self.rgt.jnts[1]['loc_pos'] = np.array([0, -0.0065, 0.0837])
self.rgt.jnts[1]['type'] = 'prismatic'
self.rgt.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.rgt.lnks[1]['name'] = "finger2"
self.rgt.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes",
"finger.stl")
self.rgt.lnks[1]['rgba'] = [.2, .2, .2, 1]
# reinitialize
self.lft.reinitialize(cdmesh_type=cdmesh_type)
self.rgt.reinitialize(cdmesh_type=cdmesh_type)
# jaw width
self.jawwidth_rng = [0.0, .05]
# jaw center
self.jaw_center_pos = np.array([0, 0, .13])
# collision detection
self.all_cdelements = []
self.enable_cc(toggle_cdprimit=enable_cc)
def set_rpy(self, roll, pitch, yaw):
"""
set the pose of the object using rpy
:param roll: radian
:param pitch: radian
:param yaw: radian
:return:
author: weiwei
date: 20190513
"""
npmat3 = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz")
self.set_rotmat(npmat3)
def loadObjitem(f_name,
pos=(0, 0, 0),
rot=(0, 0, 0),
sample_num=10000,
type="box"):
if f_name[-3:] != 'stl':
f_name += '.stl'
objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT, "obstacles",
f_name),
type=type)
objmat4 = np.zeros([4, 4])
objmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz")
objmat4[:3, 3] = pos
objcm.sethomomat(objmat4)
return item.Item(objcm=objcm, objmat4=objmat4, sample_num=sample_num)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(7), name='irb14050', enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name)
# seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1
jnt_safemargin = math.pi / 18.0
# self.jlc.jnts[1]['loc_pos'] = np.array([0.05355, -0.0725, 0.41492])
# self.jlc.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(-0.9795, -0.5682, -2.3155)
self.jlc.jnts[1]['loc_pos'] = np.array([0., 0., 0.])
self.jlc.jnts[1]['motion_rng'] = [-2.94087978961 + jnt_safemargin, 2.94087978961 - jnt_safemargin]
self.jlc.jnts[2]['loc_pos'] = np.array([0.03, 0.0, 0.1])
self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, 0.0, 0.0)
self.jlc.jnts[2]['motion_rng'] = [-2.50454747661 + jnt_safemargin, 0.759218224618 - jnt_safemargin]
self.jlc.jnts[3]['loc_pos'] = np.array([-0.03, 0.17283, 0.0])
self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0)
self.jlc.jnts[3]['motion_rng'] = [-2.94087978961 + jnt_safemargin, 2.94087978961 - jnt_safemargin]
self.jlc.jnts[4]['loc_pos'] = np.array([-0.04188, 0.0, 0.07873])
self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, -1.57079632679, 0.0)
self.jlc.jnts[4]['motion_rng'] = [-2.15548162621 + jnt_safemargin, 1.3962634016 - jnt_safemargin]
self.jlc.jnts[5]['loc_pos'] = np.array([0.0405, 0.16461, 0.0])
self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0)
self.jlc.jnts[5]['motion_rng'] = [-5.06145483078 + jnt_safemargin, 5.06145483078 - jnt_safemargin]
self.jlc.jnts[6]['loc_pos'] = np.array([-0.027, 0, 0.10039])
self.jlc.jnts[6]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, 0.0, 0.0)
self.jlc.jnts[6]['motion_rng'] = [-1.53588974176 + jnt_safemargin, 2.40855436775 - jnt_safemargin]
self.jlc.jnts[7]['loc_pos'] = np.array([0.027, 0.029, 0.0])
self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0)
self.jlc.jnts[7]['motion_rng'] = [-3.99680398707 + jnt_safemargin, 3.99680398707 - jnt_safemargin]
# links
self.jlc.lnks[1]['name'] = "link_1"
self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "link_1.stl")
self.jlc.lnks[1]['rgba'] = [.5, .5, .5, 1]
self.jlc.lnks[2]['name'] = "link_2"
self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes", "link_2.stl")
self.jlc.lnks[2]['rgba'] = [.929, .584, .067, 1]
self.jlc.lnks[3]['name'] = "link_3"
self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes", "link_3.stl")
self.jlc.lnks[3]['rgba'] = [.7, .7, .7, 1]
self.jlc.lnks[4]['name'] = "link_4"
self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "link_4.stl")
self.jlc.lnks[4]['rgba'] = [0.180, .4, 0.298, 1]
self.jlc.lnks[5]['name'] = "link_5"
self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes", "link_5.stl")
self.jlc.lnks[5]['rgba'] = [.7, .7, .7, 1]
self.jlc.lnks[6]['name'] = "link_6"
self.jlc.lnks[6]['meshfile'] = os.path.join(this_dir, "meshes", "link_6.stl")
self.jlc.lnks[6]['rgba'] = [0.180, .4, 0.298, 1]
self.jlc.lnks[7]['name'] = "link_7"
# self.jlc.lnks[7]['meshfile'] = os.path.join(this_dir, "meshes", "link_7.stl") # not really needed to visualize
# self.jlc.lnks[7]['rgba'] = [.5,.5,.5,1]
# reinitialization
self.jlc.reinitialize()
# collision detection
if enable_cc:
self.enable_cc()
def setRPY(self, roll, pitch, yaw):
"""
set the pose of the object using rpy
:param roll: degree
:param pitch: degree
:param yaw: degree
:return:
author: weiwei
date: 20190513
"""
currentmat = self._bdb.gethomomat()
# currentmatnp = base.pg.mat4ToNp(currentmat)# motified by hu 2020/06/30
currentmatnp = currentmat # motified by hu 2020/06/30
newmatnp = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz")
self.set(base.pg.npToMat4(newmatnp, currentmatnp[:, 3]))
def set_rpy(self, roll, pitch, yaw):
"""
set the pose of the object using rpy
:param roll: degree
:param pitch: degree
:param yaw: degree
:return:
author: weiwei
date: 20190513
"""
currentmatnp = self._gm.get_homomat()
# currentmatnp = da.pdmat4_to_npmat4(currentmat)
newmatnp = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz")
# self.__objnp.setMat(p3du.npToMat4(newmatnp, currentmatnp[:,3]))
self._bdb.set_homomat(
rm.homomat_from_posrot(currentmatnp[:3, 3], newmatnp))
self._gm.set_homomat(
rm.homomat_from_posrot(currentmatnp[:3, 3], newmatnp))
def loadObjpcd(f_name,
pos=(0, 0, 0),
rot=(0, 0, 0),
sample_num=100000,
toggledebug=False):
obj = cm.CollisionModel(
objinit=os.path.join(config.ROOT, "obstacles", f_name))
rotmat4 = np.zeros([4, 4])
rotmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz")
rotmat4[:3, 3] = pos
obj_surface = np.asarray(obj.sample_surface(nsample=sample_num))
# obj_surface = obj_surface[obj_surface[:, 2] > 2]
obj_surface_real = __pcd_trans(obj_surface, rotmat4)
if toggledebug:
# obj_surface = o3d_helper.nparray2o3dpcd(copy.deepcopy(obj_surface))
# obj_surface.paint_uniform_color([1, 0.706, 0])
# o3d.visualization.draw_geometries([obj_surface], window_name='loadObjpcd')
# pcddnp = base.pg.genpointcloudnp(obj_surface_real)
# pcddnp.reparentTo(base.render)
pass
return obj_surface_real
def update(bunnycm, task):
if base.inputmgr.keymap['space'] is True:
for i in range(1):
bunnycm1 = bunnycm.copy()
bunnycm1.set_mass(.1)
rndcolor = np.random.rand(4)
rndcolor[-1] = 1
bunnycm1.set_rgba(rndcolor)
rotmat = rm.rotmat_from_euler(0, 0, math.pi / 12)
z = math.floor(i / 100)
y = math.floor((i - z * 100) / 10)
x = i - z * 100 - y * 10
print(x, y, z, "\n")
bunnycm1.set_homomat(
rm.homomat_from_posrot(
np.array(
[x * 0.015 - 0.07, y * 0.015 - 0.07,
0.35 + z * 0.015]), rotmat))
base.attach_internal_update_obj(bunnycm1)
bunnycm1.start_physics()
base.inputmgr.keymap['space'] = False
return task.cont
def update(bunnycm, task):
if base.inputmgr.keyMap['space'] is True:
for i in range(300):
bunnycm1 = bunnycm.copy()
bunnycm1.setMass(.1)
# bunnycm1.setColor(0.7, 0, 0.7, 1.0)
bunnycm1.setColor(random.random(), random.random(),
random.random(), 1.0)
# bunnycm1.reparentTo(base.render)
# rotmat = rm.rodrigues([0,0,1], 15)
rotmat = rm.rotmat_from_euler(0, 0, 15)
z = math.floor(i / 100)
y = math.floor((i - z * 100) / 10)
x = i - z * 100 - y * 10
print(x, y, z, "\n")
bunnycm1.setMat(
base.pg.npToMat4(
rotmat,
np.array([x * 15 - 70, y * 15 - 70, 150 + z * 15])))
base.attachRUD(bunnycm1)
base.inputmgr.keyMap['space'] = False
return task.cont
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
name='ur3edual',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# left side
self.lft_body = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(0),
name='lft_body_jl')
self.lft_body.jnts[1]['loc_pos'] = np.array(
[.365, .345, 1.33]) # left from robot_s view
self.lft_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
math.pi / 2.0, 0, math.pi / 2.0) # left from robot_s view
self.lft_body.lnks[0]['name'] = "ur3e_dual_base"
self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.lft_body.lnks[0]['collision_model'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3e_dual_base.stl"),
cdprimit_type="user_defined",
expand_radius=.005,
userdefined_cdprimitive_fn=self._base_combined_cdnp)
self.lft_body.lnks[0]['rgba'] = [.55, .55, .55, 1.0]
self.lft_body.reinitialize()
lft_arm_homeconf = np.zeros(6)
lft_arm_homeconf[0] = -math.pi * 2.0 / 3.0
lft_arm_homeconf[1] = -math.pi * 2.0 / 3.0
lft_arm_homeconf[2] = math.pi / 2.0
lft_arm_homeconf[3] = math.pi
lft_arm_homeconf[4] = -math.pi / 2.0
self.lft_arm = ur.UR3E(pos=self.lft_body.jnts[-1]['gl_posq'],
rotmat=self.lft_body.jnts[-1]['gl_rotmatq'],
homeconf=lft_arm_homeconf,
enable_cc=False)
self.lft_hnd = rtq.RobotiqHE(
pos=self.lft_arm.jnts[-1]['gl_posq'],
rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'],
coupling_offset_pos=np.array([0.0, 0.0, 0.0331]),
enable_cc=False)
# rigth side
self.rgt_body = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(0),
name='rgt_body_jl')
self.rgt_body.jnts[1]['loc_pos'] = np.array(
[.365, -.345, 1.33]) # right from robot_s view
self.rgt_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
math.pi / 2.0, 0, math.pi / 2.0) # left from robot_s view
self.rgt_body.lnks[0]['name'] = "ur3e_dual_base"
self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.rgt_body.lnks[0]['mesh_file'] = None
self.rgt_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0]
self.rgt_body.reinitialize()
rgt_arm_homeconf = np.zeros(6)
rgt_arm_homeconf[0] = math.pi * 2.0 / 3.0
rgt_arm_homeconf[1] = -math.pi / 3.0
rgt_arm_homeconf[2] = -math.pi / 2.0
rgt_arm_homeconf[4] = math.pi / 2.0
self.rgt_arm = ur.UR3E(pos=self.rgt_body.jnts[-1]['gl_posq'],
rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'],
homeconf=rgt_arm_homeconf,
enable_cc=False)
self.rgt_hnd = rtq.RobotiqHE(
pos=self.rgt_arm.jnts[-1]['gl_posq'],
rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'],
coupling_offset_pos=np.array([0.0, 0.0, 0.0331]),
enable_cc=False)
# tool center point
# lft
self.lft_arm.tcp_jnt_id = -1
self.lft_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos
self.lft_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat
# rgt
self.rgt_arm.tcp_jnt_id = -1
self.rgt_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos
self.rgt_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.lft_oih_infos = []
self.rgt_oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
# component map
self.manipulator_dict['rgt_arm'] = self.rgt_arm
self.manipulator_dict['lft_arm'] = self.lft_arm
self.manipulator_dict[
'rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to
self.manipulator_dict[
'lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to
self.hnd_dict['rgt_hnd'] = self.rgt_hnd
self.hnd_dict['lft_hnd'] = self.lft_hnd
self.hnd_dict['rgt_arm'] = self.rgt_hnd
self.hnd_dict['lft_arm'] = self.lft_hnd
object1.set_pos(np.array([.45, .48, 1.11]))
object1.set_rgba([.8, .8, .8, 1])
object1.attach_to(base)
# マグカップの生成
object2 = cm.CollisionModel("./objects/mug.stl")
object2.set_pos(np.array([.3, .48, 1.11]))
object2.set_rgba([.5, .7, .3, 1])
object2.attach_to(base)
# ロボットの生成
component_name = 'lft_arm'
robot_s = ur3d.UR3Dual()
start_pos = np.array([.4, .3, 1.11])
start_rotmat = rm.rotmat_from_euler(ai=math.pi / 2,
aj=math.pi,
ak=0,
axes='szxz')
start_conf = robot_s.ik(component_name=component_name,
tgt_pos=start_pos,
tgt_rotmat=start_rotmat)
goal_pos = np.array([.4, .7, 1.11])
goal_rotmat = rm.rotmat_from_euler(ai=math.pi / 2,
aj=math.pi,
ak=0,
axes='szxz')
goal_conf = robot_s.ik(component_name=component_name,
tgt_pos=goal_pos,
tgt_rotmat=goal_rotmat)
# 経路計画
rrtc_planner = rrtc.RRTConnect(robot_s)
self.rgt.gen_meshmodel(tcp_loc_pos=None,
tcp_loc_rotmat=None,
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()
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)
bunnycm.attach_to(base)
bunnycm1.attach_to(base)
bunnycm2.attach_to(base)
bunnycm.show_cdprimit()
bunnycm1.show_cdprimit()
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
name='nextage',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
central_homeconf = np.radians(np.array([.0, .0, .0]))
lft_arm_homeconf = np.radians(
np.array([central_homeconf[0], 15, 0, -143, 0, 0, 0]))
rgt_arm_homeconf = np.radians(
np.array([central_homeconf[0], -15, 0, -143, 0, 0, 0]))
# central
self.central_body = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=central_homeconf,
name='centeral_body')
self.central_body.jnts[1]['loc_pos'] = np.array([0, 0, 0])
self.central_body.jnts[1]['loc_motionax'] = np.array([0, 0, 1])
self.central_body.jnts[1]['motion_rng'] = [-2.84489, 2.84489]
self.central_body.jnts[2]['loc_pos'] = np.array([0, 0, 0.5695])
self.central_body.jnts[2]['loc_motionax'] = np.array([0, 0, 1])
self.central_body.jnts[2]['motion_rng'] = [-1.22173, 1.22173]
self.central_body.jnts[3]['loc_pos'] = np.array([0, 0, 0])
self.central_body.jnts[3]['loc_motionax'] = np.array([0, 1, 0])
self.central_body.jnts[3]['motion_rng'] = [-0.349066, 1.22173]
self.central_body.lnks[0]['name'] = "nextage_base"
self.central_body.lnks[0]['loc_pos'] = np.array([0, 0, 0.97])
self.central_body.lnks[0]['collision_model'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "waist_link_mesh.dae"),
cdprimit_type="user_defined",
expand_radius=.005,
userdefined_cdprimitive_fn=self._waist_combined_cdnp)
self.central_body.lnks[0]['rgba'] = [.77, .77, .77, 1.0]
self.central_body.lnks[1]['name'] = "nextage_chest"
self.central_body.lnks[1]['loc_pos'] = np.array([0, 0, 0])
self.central_body.lnks[1]['collision_model'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "chest_joint0_link_mesh.dae"),
cdprimit_type="user_defined",
expand_radius=.005,
userdefined_cdprimitive_fn=self._chest_combined_cdnp)
self.central_body.lnks[1]['rgba'] = [1, .65, .5, 1]
self.central_body.lnks[2]['name'] = "head_joint0_link_mesh"
self.central_body.lnks[2]['loc_pos'] = np.array([0, 0, 0.5695])
self.central_body.lnks[2]['mesh_file'] = os.path.join(
this_dir, "meshes", "head_joint0_link_mesh.dae")
self.central_body.lnks[2]['rgba'] = [.35, .35, .35, 1]
self.central_body.lnks[3]['name'] = "nextage_chest"
self.central_body.lnks[3]['loc_pos'] = np.array([0, 0, 0])
self.central_body.lnks[3]['mesh_file'] = os.path.join(
this_dir, "meshes", "head_joint1_link_mesh.dae")
self.central_body.lnks[3]['rgba'] = [.63, .63, .63, 1]
self.central_body.reinitialize()
# lft
self.lft_arm = jl.JLChain(
pos=self.central_body.jnts[1]['gl_posq'],
rotmat=self.central_body.jnts[1]['gl_rotmatq'],
homeconf=lft_arm_homeconf,
name='lft_arm')
self.lft_arm.jnts[1]['loc_pos'] = np.array([0, 0, 0])
self.lft_arm.jnts[1]['loc_motionax'] = np.array([0, 0, 1])
self.lft_arm.jnts[2]['loc_pos'] = np.array([0, 0.145, 0.370296])
self.lft_arm.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(
-0.261799, 0, 0)
self.lft_arm.jnts[2]['loc_motionax'] = np.array([0, 0, 1])
self.lft_arm.jnts[2]['motion_rng'] = [-1.53589, 1.53589]
self.lft_arm.jnts[3]['loc_pos'] = np.array([0, 0, 0])
self.lft_arm.jnts[3]['loc_motionax'] = np.array([0, 1, 0])
self.lft_arm.jnts[3]['motion_rng'] = [-2.44346, 1.0472]
self.lft_arm.jnts[4]['loc_pos'] = np.array([0, 0.095, -0.25])
self.lft_arm.jnts[4]['loc_motionax'] = np.array([0, 1, 0])
self.lft_arm.jnts[4]['motion_rng'] = [-2.75762, 0]
self.lft_arm.jnts[5]['loc_pos'] = np.array([-0.03, 0, 0])
self.lft_arm.jnts[5]['loc_motionax'] = np.array([0, 0, 1])
self.lft_arm.jnts[5]['motion_rng'] = [-1.8326, 2.87979]
self.lft_arm.jnts[6]['loc_pos'] = np.array([0, 0, -0.235])
self.lft_arm.jnts[6]['loc_motionax'] = np.array([0, 1, 0])
self.lft_arm.jnts[6]['motion_rng'] = [-1.74533, 1.74533]
self.lft_arm.jnts[7]['loc_pos'] = np.array([-0.047, 0, -0.09])
self.lft_arm.jnts[7]['loc_motionax'] = np.array([1, 0, 0])
self.lft_arm.jnts[7]['motion_rng'] = [-2.84489, 2.84489]
self.lft_arm.lnks[2]['name'] = "lft_arm_joint0"
self.lft_arm.lnks[2]['loc_pos'] = np.array([0, 0.145, 0.370296])
self.lft_arm.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler(
-0.261799, 0, 0)
self.lft_arm.lnks[2]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint0_link_mesh.dae")
self.lft_arm.lnks[2]['rgba'] = [.35, .35, .35, 1]
self.lft_arm.lnks[3]['name'] = "lft_arm_joint1"
self.lft_arm.lnks[3]['loc_pos'] = np.array([0, 0, 0])
self.lft_arm.lnks[3]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint1_link_mesh.dae")
self.lft_arm.lnks[3]['rgba'] = [.57, .57, .57, 1]
self.lft_arm.lnks[4]['name'] = "lft_arm_joint2"
self.lft_arm.lnks[4]['loc_pos'] = np.array([0, 0.095, -0.25])
self.lft_arm.lnks[4]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint2_link_mesh.dae")
self.lft_arm.lnks[4]['rgba'] = [.35, .35, .35, 1]
self.lft_arm.lnks[5]['name'] = "lft_arm_joint3"
self.lft_arm.lnks[5]['loc_pos'] = np.array([-0.03, 0, 0])
self.lft_arm.lnks[5]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint3_link_mesh.dae")
self.lft_arm.lnks[5]['rgba'] = [.35, .35, .35, 1]
self.lft_arm.lnks[6]['name'] = "lft_arm_joint4"
self.lft_arm.lnks[6]['loc_pos'] = np.array([0, 0, -0.235])
self.lft_arm.lnks[6]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint4_link_mesh.dae")
self.lft_arm.lnks[6]['rgba'] = [.7, .7, .7, 1]
self.lft_arm.lnks[7]['name'] = "lft_arm_joint5"
self.lft_arm.lnks[7]['loc_pos'] = np.array([-0.047, 0, -0.09])
self.lft_arm.lnks[7]['mesh_file'] = os.path.join(
this_dir, "meshes", "larm_joint5_link_mesh.dae")
self.lft_arm.lnks[7]['rgba'] = [.57, .57, .57, 1]
self.lft_arm.reinitialize()
# rgt
self.rgt_arm = jl.JLChain(
pos=self.central_body.jnts[1]['gl_posq'],
rotmat=self.central_body.jnts[1]['gl_rotmatq'],
homeconf=rgt_arm_homeconf,
name='rgt_arm')
self.rgt_arm.jnts[1]['loc_pos'] = np.array([0, 0, 0])
self.rgt_arm.jnts[1]['loc_motionax'] = np.array([0, 0, 1])
self.rgt_arm.jnts[2]['loc_pos'] = np.array([0, -0.145, 0.370296])
self.rgt_arm.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(
0.261799, 0, 0)
self.rgt_arm.jnts[2]['loc_motionax'] = np.array([0, 0, 1])
self.rgt_arm.jnts[2]['motion_rng'] = [-1.53589, 1.53589]
self.rgt_arm.jnts[3]['loc_pos'] = np.array([0, 0, 0])
self.rgt_arm.jnts[3]['loc_motionax'] = np.array([0, 1, 0])
self.rgt_arm.jnts[3]['motion_rng'] = [-2.44346, 1.0472]
self.rgt_arm.jnts[4]['loc_pos'] = np.array([0, -0.095, -0.25])
self.rgt_arm.jnts[4]['loc_motionax'] = np.array([0, 1, 0])
self.rgt_arm.jnts[4]['motion_rng'] = [-2.75762, 0]
self.rgt_arm.jnts[5]['loc_pos'] = np.array([-0.03, 0, 0])
self.rgt_arm.jnts[5]['loc_motionax'] = np.array([0, 0, 1])
self.rgt_arm.jnts[5]['motion_rng'] = [-1.8326, 2.87979]
self.rgt_arm.jnts[6]['loc_pos'] = np.array([0, 0, -0.235])
self.rgt_arm.jnts[6]['loc_motionax'] = np.array([0, 1, 0])
self.rgt_arm.jnts[6]['motion_rng'] = [-1.74533, 1.74533]
self.rgt_arm.jnts[7]['loc_pos'] = np.array([-0.047, 0, -0.09])
self.rgt_arm.jnts[7]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_arm.jnts[7]['motion_rng'] = [-2.84489, 2.84489]
self.rgt_arm.lnks[2]['name'] = "rgt_arm_joint0"
self.rgt_arm.lnks[2]['loc_pos'] = np.array([0, -0.145, 0.370296])
self.rgt_arm.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler(
0.261799, 0, 0)
self.rgt_arm.lnks[2]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint0_link_mesh.dae")
self.rgt_arm.lnks[2]['rgba'] = [.35, .35, .35, 1]
self.rgt_arm.lnks[3]['name'] = "rgt_arm_joint1"
self.rgt_arm.lnks[3]['loc_pos'] = np.array([0, 0, 0])
self.rgt_arm.lnks[3]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint1_link_mesh.dae")
self.rgt_arm.lnks[3]['rgba'] = [.57, .57, .57, 1]
self.rgt_arm.lnks[4]['name'] = "rgt_arm_joint2"
self.rgt_arm.lnks[4]['loc_pos'] = np.array([0, -0.095, -0.25])
self.rgt_arm.lnks[4]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint2_link_mesh.dae")
self.rgt_arm.lnks[4]['rgba'] = [.35, .35, .35, 1]
self.rgt_arm.lnks[5]['name'] = "rgt_arm_joint3"
self.rgt_arm.lnks[5]['loc_pos'] = np.array([-0.03, 0, 0])
self.rgt_arm.lnks[5]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint3_link_mesh.dae")
self.rgt_arm.lnks[5]['rgba'] = [.35, .35, .35, 1]
self.rgt_arm.lnks[6]['name'] = "rgt_arm_joint4"
self.rgt_arm.lnks[6]['loc_pos'] = np.array([0, 0, -0.235])
self.rgt_arm.lnks[6]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint4_link_mesh.dae")
self.rgt_arm.lnks[6]['rgba'] = [.7, .7, .7, 1]
self.rgt_arm.lnks[7]['name'] = "rgt_arm_joint5"
self.rgt_arm.lnks[7]['loc_pos'] = np.array([-0.047, 0, -0.09])
self.rgt_arm.lnks[7]['mesh_file'] = os.path.join(
this_dir, "meshes", "rarm_joint5_link_mesh.dae")
self.rgt_arm.lnks[7]['rgba'] = [.57, .57, .57, 1]
self.rgt_arm.reinitialize()
# tool center point
# lft
self.lft_arm.tcp_jnt_id = -1
# self.lft_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos
# self.lft_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat
self.lft_arm.tcp_loc_pos = np.zeros(3)
self.lft_arm.tcp_loc_rotmat = np.eye(3)
# rgt
self.rgt_arm.tcp_jnt_id = -1
# self.rgt_arm.tcp_loc_pos = self.rgt_hnd.jaw_center_pos
# self.rgt_arm.tcp_loc_rotmat = self.rgt_hnd.jaw_center_rotmat
self.rgt_arm.tcp_loc_pos = np.zeros(3)
self.rgt_arm.tcp_loc_rotmat = np.eye(3)
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.lft_oih_infos = []
self.rgt_oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
# component map
self.manipulator_dict['rgt_arm'] = self.rgt_arm
self.manipulator_dict['lft_arm'] = self.lft_arm
self.manipulator_dict['rgt_arm_waist'] = self.rgt_arm
self.manipulator_dict['lft_arm_waist'] = self.lft_arm
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
cdmesh_type='box',
name='robotiq85',
enable_cc=True):
super().__init__(pos=pos,
rotmat=rotmat,
cdmesh_type=cdmesh_type,
name=name)
this_dir, this_filename = os.path.split(__file__)
cpl_end_pos = self.coupling.jnts[-1]['gl_posq']
cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq']
# - lft_outer
self.lft_outer = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(4),
name='lft_outer')
self.lft_outer.jnts[1]['loc_pos'] = np.array([0, -.0306011, .054904])
self.lft_outer.jnts[1]['motion_rng'] = [.0, .8]
self.lft_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
0, 0, math.pi)
self.lft_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.lft_outer.jnts[2]['loc_pos'] = np.array([0, .0315,
-.0041]) # passive
self.lft_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0])
self.lft_outer.jnts[3]['loc_pos'] = np.array([0, .0061, .0471])
self.lft_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0])
self.lft_outer.jnts[4]['loc_pos'] = np.zeros(3)
# https://github.com/Danfoa uses geometry instead of the dae mesh. The following coordiante is needed
# self.lft_outer.jnts[4]['loc_pos'] = np.array([0, -0.0220203446692936, .03242])
# - lft_inner
self.lft_inner = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(1),
name='lft_inner')
self.lft_inner.jnts[1]['loc_pos'] = np.array([0, -.0127, .06142])
self.lft_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
0, 0, math.pi)
self.lft_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
# - rgt_outer
self.rgt_outer = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(4),
name='rgt_outer')
self.rgt_outer.jnts[1]['loc_pos'] = np.array([0, .0306011, .054904])
self.rgt_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[2]['loc_pos'] = np.array([0, .0315,
-.0041]) # passive
self.rgt_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[3]['loc_pos'] = np.array([0, .0061, .0471])
self.rgt_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[4]['loc_pos'] = np.zeros(3)
# https://github.com/Danfoa uses geometry instead of the dae mesh. The following coordiante is needed
# self.rgt_outer.jnts[4]['loc_pos'] = np.array([0, -0.0220203446692936, .03242])
# - rgt_inner
self.rgt_inner = jl.JLChain(pos=cpl_end_pos,
rotmat=cpl_end_rotmat,
homeconf=np.zeros(1),
name='rgt_inner')
self.rgt_inner.jnts[1]['loc_pos'] = np.array([0, .0127, .06142])
self.rgt_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
# links
# - lft_outer
self.lft_outer.lnks[0]['name'] = "robotiq85_gripper_base"
self.lft_outer.lnks[0]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[0]['com'] = np.array(
[8.625e-08, -4.6583e-06, 0.03145])
self.lft_outer.lnks[0]['mass'] = 0.22652
self.lft_outer.lnks[0]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_base_link_cvt.stl")
self.lft_outer.lnks[0]['rgba'] = [.2, .2, .2, 1]
self.lft_outer.lnks[1]['name'] = "left_outer_knuckle"
self.lft_outer.lnks[1]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[1]['com'] = np.array(
[-0.000200000000003065, 0.0199435877845359, 0.0292245259211331])
self.lft_outer.lnks[1]['mass'] = 0.00853198276973456
self.lft_outer.lnks[1]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_outer_knuckle.stl")
self.lft_outer.lnks[1]['rgba'] = [
0.792156862745098, 0.819607843137255, 0.933333333333333, 1
]
self.lft_outer.lnks[2]['name'] = "left_outer_finger"
self.lft_outer.lnks[2]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[2]['com'] = np.array(
[0.00030115855001899, 0.0373907951953854, -0.0208027427000385])
self.lft_outer.lnks[2]['mass'] = 0.022614240507152
self.lft_outer.lnks[2]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_outer_finger_cvt.stl")
self.lft_outer.lnks[2]['rgba'] = [.2, .2, .2, 1]
self.lft_outer.lnks[3]['name'] = "left_inner_finger"
self.lft_outer.lnks[3]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[3]['com'] = np.array(
[0.000299999999999317, 0.0160078233491243, -0.0136945669206257])
self.lft_outer.lnks[3]['mass'] = 0.0104003125914103
self.lft_outer.lnks[3]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_85_inner_finger_gelsight_cvt.stl")
self.lft_outer.lnks[3]['rgba'] = [0.57, 0.57, 0.57, 1]
self.lft_outer.lnks[4]['name'] = "left_inner_finger_pad"
self.lft_outer.lnks[4]['loc_pos'] = np.zeros(3)
# self.lft_outer.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_85_pad.dae")
# self.lft_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3]
# self.lft_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
# - lft_inner
self.lft_inner.lnks[1]['name'] = "left_inner_knuckle"
self.lft_inner.lnks[1]['loc_pos'] = np.zeros(3)
self.lft_inner.lnks[1]['com'] = np.array(
[0.000123011831763771, 0.0507850843201817, 0.00103968640075166])
self.lft_inner.lnks[1]['mass'] = 0.0271177346495152
self.lft_inner.lnks[1]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_inner_knuckle_cvt.stl")
self.lft_inner.lnks[1]['rgba'] = [.2, .2, .2, 1]
# - rgt_outer
self.rgt_outer.lnks[1]['name'] = "left_outer_knuckle"
self.rgt_outer.lnks[1]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[1]['com'] = np.array(
[-0.000200000000003065, 0.0199435877845359, 0.0292245259211331])
self.rgt_outer.lnks[1]['mass'] = 0.00853198276973456
self.rgt_outer.lnks[1]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_outer_knuckle.stl")
self.rgt_outer.lnks[1]['rgba'] = [
0.792156862745098, 0.819607843137255, 0.933333333333333, 1
]
self.rgt_outer.lnks[2]['name'] = "left_outer_finger"
self.rgt_outer.lnks[2]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[2]['com'] = np.array(
[0.00030115855001899, 0.0373907951953854, -0.0208027427000385])
self.rgt_outer.lnks[2]['mass'] = 0.022614240507152
self.rgt_outer.lnks[2]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_outer_finger_cvt.stl")
self.rgt_outer.lnks[2]['rgba'] = [.2, .2, .2, 1]
self.rgt_outer.lnks[3]['name'] = "left_inner_finger"
self.rgt_outer.lnks[3]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[3]['com'] = np.array(
[0.000299999999999317, 0.0160078233491243, -0.0136945669206257])
self.rgt_outer.lnks[3]['mass'] = 0.0104003125914103
self.rgt_outer.lnks[3]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_85_inner_finger_gelsight_cvt.stl")
self.rgt_outer.lnks[3]['rgba'] = [0.57, 0.57, 0.57, 1]
self.rgt_outer.lnks[4]['name'] = "left_inner_finger_pad"
self.rgt_outer.lnks[4]['loc_pos'] = np.zeros(3)
# self.rgt_outer.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_85_pad.dae")
# self.rgt_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3]
# self.rgt_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
# - rgt_inner
self.rgt_inner.lnks[1]['name'] = "left_inner_knuckle"
self.rgt_inner.lnks[1]['loc_pos'] = np.zeros(3)
self.rgt_inner.lnks[1]['com'] = np.array(
[0.000123011831763771, 0.0507850843201817, 0.00103968640075166])
self.rgt_inner.lnks[1]['mass'] = 0.0271177346495152
self.rgt_inner.lnks[1]['meshfile'] = os.path.join(
this_dir, "meshes", "robotiq_arg2f_85_inner_knuckle_cvt.stl")
self.rgt_inner.lnks[1]['rgba'] = [.2, .2, .2, 1]
# reinitialize
self.lft_outer.reinitialize()
self.lft_inner.reinitialize()
self.rgt_outer.reinitialize()
self.rgt_inner.reinitialize()
# jaw width
self.jawwidth_rng = [0.0, .085]
# jaw center
self.jaw_center_pos = np.array([0, 0, .180])
# collision detection
self.all_cdelements = []
self.enable_cc(toggle_cdprimit=enable_cc)
import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xsm
import manipulation.pick_place_planner as ppp
import motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[2.1, -2.1, 2.1], lookat_pos=[.0, 0, .3])
gm.gen_frame().attach_to(base)
# ground
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7])
ground.set_pos(np.array([0, 0, -.51]))
ground.attach_to(base)
# object box
object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7])
# object_box_gl_pos = np.array([.3, -.4, .35])
# object_box_gl_rotmat = np.eye(3)
object_box_gl_pos = np.array([.3, .4, .35])
object_box_gl_rotmat = rm.rotmat_from_euler(0, 0, math.pi / 3)
obgl_start_homomat = rm.homomat_from_posrot(object_box_gl_pos,
object_box_gl_rotmat)
object_box.set_pos(object_box_gl_pos)
object_box.set_rotmat(object_box_gl_rotmat)
gm.gen_frame().attach_to(object_box)
object_box_copy = object_box.copy()
# object_box_copy.attach_to(base)
# object box goal
# object_box_gl_goal_pos = np.array([.6, -.1, .1])
# object_box_gl_goal_rotmat = rm.rotmat_from_euler(0, math.pi / 2, math.pi / 2)
object_box_gl_goal_pos = np.array([.3, -.4, .1])
object_box_gl_goal_rotmat = rm.rotmat_from_euler(math.pi / 3, math.pi / 2,
math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_box_gl_goal_pos,
object_box_gl_goal_rotmat)
ground.attach_to(base)
# object holder
object_holder = cm.CollisionModel("objects/holder.stl")
object_holder.set_rgba([.5, .5, .5, 1])
object_holder_gl_pos = np.array([-.15, -.3, .0])
object_holder_gl_rotmat = np.eye(3)
obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos,
object_holder_gl_rotmat)
object_holder.set_pos(object_holder_gl_pos)
object_holder.set_rotmat(object_holder_gl_rotmat)
gm.gen_frame().attach_to(object_holder)
object_holder_copy = object_holder.copy()
object_holder_copy.attach_to(base)
# object holder goal
object_holder_gl_goal_pos = np.array([.25, -.05, .05])
object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos,
object_holder_gl_goal_rotmat)
object_holder_goal_copy = object_holder.copy()
object_holder_goal_copy.set_homomat(obgl_goal_homomat)
object_holder_goal_copy.attach_to(base)
robot_s = cbt.Cobotta()
# robot_s.gen_meshmodel().attach_to(base)
rrtc_s = rrtc.RRTConnect(robot_s)
ppp_s = ppp.PickPlacePlanner(robot_s)
original_grasp_info_list = gpa.load_pickle_file('holder', './',
'cobg_holder_grasps.pickle')
manipulator_name = "arm"
hand_name = "hnd"
base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
object = cm.CollisionModel("./objects/bunnysim.stl")
object.set_pos(np.array([.85, 0, .37]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
# robot_s
component_name = 'arm'
robot_s = xss.XArmShuidi()
robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
# base.run()
seed_jnt_values = robot_s.get_jnt_values(component_name=component_name)
for y in range(-5, 5):
tgt_pos = np.array([.3, y * .1, 1])
tgt_rotmat = rm.rotmat_from_euler(0, math.pi / 2, 0)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
tic = time.time()
jnt_values = robot_s.ik(component_name=component_name,
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat,
max_niter=500,
toggle_debug=False,
seed_jnt_values=seed_jnt_values)
toc = time.time()
print(f"time cost: {toc-tic}")
seed_jnt_values = jnt_values
if jnt_values is not None:
robot_s.fk(component_name=component_name, jnt_values=jnt_values)
robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
base.run()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="tbm"):
self.pos = pos
self.rotmat = rotmat
self.name = name
this_dir, this_filename = os.path.split(__file__)
# head
self.head = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(1),
name='head')
self.head.jnts[1]['loc_pos'] = np.zeros(3)
self.head.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.head.lnks[0]['name'] = 'tbm_front_shield'
self.head.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.head.lnks[0]['meshfile'] = os.path.join(this_dir, 'meshes',
'tbm_front_shield.stl')
self.head.lnks[0]['rgba'] = [.7, .7, .7, .3]
self.head.lnks[1]['name'] = 'tbm_cutter_head'
self.head.lnks[1]['loc_pos'] = np.array([0, 0, 0])
self.head.lnks[1]['meshfile'] = os.path.join(this_dir, 'meshes',
'tbm_cutter_head.stl')
self.head.lnks[1]['rgba'] = [.7, .3, .3, 1]
self.head.tgtjnts = [1]
self.head.reinitialize()
# cutter
self.cutters = []
self.cutter_pos_dict = {
'0': [
np.array([0.410, 0.0, 0.341 + 0.175]),
np.array([0.410, 0.0, 0.590 + 0.175]),
np.array([0.410, 0.0, 0.996 + 0.175]),
np.array([0.410, 0.0, 1.245 + 0.175]),
np.array([0.410, 0.0, 2.162 + 0.175]),
np.array([0.410, 0.0, 2.802 + 0.175]),
np.array([0.410, 0.0, 3.442 + 0.175]),
np.array([0.410, 0.0, 4.082 + 0.175]),
np.array([0.410, 0.0, 4.662 + 0.175]),
np.array([0.410, 0.0, 5.712 + 0.175])
],
'6': [
np.array([0.410, 0.0, -0.341 - 0.175]),
np.array([0.410, 0.0, -0.590 - 0.175]),
np.array([0.410, 0.0, -0.996 - 0.175]),
np.array([0.410, 0.0, -1.245 - 0.175]),
np.array([0.410, 0.0, -2.020 - 0.175]),
np.array([0.410, 0.0, -2.660 - 0.175]),
np.array([0.410, 0.0, -3.300 - 0.175]),
np.array([0.410, 0.0, -3.915 - 0.175]),
np.array([0.410, 0.0, -4.515 - 0.175]),
np.array([0.410, 0.0, -5.560 - 0.175])
],
'3': [
np.array([0.410, -0.050 + 0.175, 0.0]),
np.array([0.410, 0.177 + 0.175, 0.0]),
np.array([0.410, 0.605 + 0.175, 0.0]),
np.array([0.410, 0.854 + 0.175, 0.0]),
np.array([0.410, 1.450 + 0.175, 0.0]),
np.array([0.410, 1.699 + 0.175, 0.0]),
np.array([0.410, 2.562 + 0.175, 0.0]),
np.array([0.410, 3.202 + 0.175, 0.0]),
np.array([0.410, 3.837 + 0.175, 0.0]),
np.array([0.410, 4.437 + 0.175, 0.0]),
np.array([0.410, 5.037 + 0.175, 0.0]),
np.array([0.410, 5.862 + 0.175, 0.0]),
np.array([0.410, 6.912 + 0.175, 0.0])
],
'9': [
np.array([0.410, 0.050 - 0.175, 0.0]),
np.array([0.410, -0.177 - 0.175, 0.0]),
np.array([0.410, -0.605 - 0.175, 0.0]),
np.array([0.410, -0.854 - 0.175, 0.0]),
np.array([0.410, -1.450 - 0.175, 0.0]),
np.array([0.410, -1.699 - 0.175, 0.0]),
np.array([0.410, -2.260 - 0.175, 0.0]),
np.array([0.410, -2.900 - 0.175, 0.0]),
np.array([0.410, -3.540 - 0.175, 0.0]),
np.array([0.410, -4.140 - 0.175, 0.0]),
np.array([0.410, -4.740 - 0.175, 0.0]),
np.array([0.410, -5.715 - 0.175, 0.0]),
np.array([0.410, -6.765 - 0.175, 0.0]),
np.array([
0.417 - 0.175 * math.sin(math.pi / 6),
-7.637 - 0.175 * math.cos(math.pi / 6), 0.0
])
],
'1.5': [
np.array([
0.410, 1.259 + 0.175 * math.sin(math.pi / 4),
1.259 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 1.711 + 0.175 * math.sin(math.pi / 4),
1.711 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 2.164 + 0.175 * math.sin(math.pi / 4),
2.164 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 2.609 + 0.175 * math.sin(math.pi / 4),
2.609 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 3.033 + 0.175 * math.sin(math.pi / 4),
3.033 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 3.882 + 0.175 * math.sin(math.pi / 4),
3.882 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 4.731 + 0.175 * math.sin(math.pi / 4),
4.731 + 0.175 * math.cos(math.pi / 4)
]),
np.array([
0.410, 5.101 + 0.175 * math.sin(math.pi / 4),
5.101 + 0.175 * math.cos(math.pi / 4)
])
],
'4.5': [
np.array([
0.410, 1.541 + 0.175 * math.sin(3 * math.pi / 4),
-1.541 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 1.994 + 0.175 * math.sin(3 * math.pi / 4),
-1.994 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 2.447 + 0.175 * math.sin(3 * math.pi / 4),
-2.447 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 2.874 + 0.175 * math.sin(3 * math.pi / 4),
-2.874 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 3.299 + 0.175 * math.sin(3 * math.pi / 4),
-3.299 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 4.253 + 0.175 * math.sin(3 * math.pi / 4),
-4.253 + 0.175 * math.cos(3 * math.pi / 4)
]),
np.array([
0.410, 4.996 + 0.175 * math.sin(3 * math.pi / 4),
-4.996 + 0.175 * math.cos(3 * math.pi / 4)
])
],
'7.5': [
np.array([
0.410, -1.315 + 0.175 * math.sin(-3 * math.pi / 4),
-1.315 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -1.768 + 0.175 * math.sin(-3 * math.pi / 4),
-1.768 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -2.220 + 0.175 * math.sin(-3 * math.pi / 4),
-2.220 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -2.662 + 0.175 * math.sin(-3 * math.pi / 4),
-2.662 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -3.086 + 0.175 * math.sin(-3 * math.pi / 4),
-3.086 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -4.094 + 0.175 * math.sin(-3 * math.pi / 4),
-4.094 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.410, -4.837 + 0.175 * math.sin(-3 * math.pi / 4),
-4.837 + 0.175 * math.cos(-3 * math.pi / 4)
]),
np.array([
0.436 + 0.175 * math.sin(-math.pi / 4), -5.429 + 0.175 *
math.sin(-3 * math.pi / 4) * math.sin(3 * math.pi / 4),
-5.429 + 0.175 * math.cos(-3 * math.pi / 4) *
math.sin(3 * math.pi / 4)
])
],
'10.5': [
np.array([
0.410, -1.485 + 0.175 * math.sin(-math.pi / 4),
1.485 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -1.937 + 0.175 * math.sin(-math.pi / 4),
1.937 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -2.390 + 0.175 * math.sin(-math.pi / 4),
2.390 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -2.821 + 0.175 * math.sin(-math.pi / 4),
2.821 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -3.246 + 0.175 * math.sin(-math.pi / 4),
3.246 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -3.723 + 0.175 * math.sin(-math.pi / 4),
3.723 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -4.200 + 0.175 * math.sin(-math.pi / 4),
4.200 + 0.175 * math.cos(-math.pi / 4)
]),
np.array([
0.410, -4.943 + 0.175 * math.sin(-math.pi / 4),
4.943 + 0.175 * math.cos(-math.pi / 4)
])
],
'0.75': [
np.array([
0.410, 2.072 + 0.175 * math.sin(math.pi / 8),
5.003 + 0.175 * math.cos(math.pi / 8)
]),
np.array([
0.410, 2.503 + 0.175 * math.sin(math.pi / 8),
6.042 + 0.175 * math.cos(math.pi / 8)
]),
np.array([
0.410, 2.818 + 0.175 * math.sin(math.pi / 8),
6.804 + 0.175 * math.cos(math.pi / 8)
])
],
'2.25': [
np.array([
0.410, 4.656 + 0.175 * math.sin(3 * math.pi / 8),
1.929 + 0.175 * math.cos(3 * math.pi / 8)
]),
np.array([
0.410, 5.626 + 0.175 * math.sin(3 * math.pi / 8),
2.331 + 0.175 * math.cos(3 * math.pi / 8)
]),
np.array([
0.410, 6.596 + 0.175 * math.sin(3 * math.pi / 8),
2.732 + 0.175 * math.cos(3 * math.pi / 8)
])
],
'3.75': [
np.array([
0.410, 4.795 + 0.175 * math.sin(5 * math.pi / 8),
-1.986 + 0.175 * math.cos(5 * math.pi / 8)
]),
np.array([
0.410, 5.973 + 0.175 * math.sin(5 * math.pi / 8),
-2.474 + 0.175 * math.cos(5 * math.pi / 8)
]),
np.array([
0.410, 6.735 + 0.175 * math.sin(5 * math.pi / 8),
-2.790 + 0.175 * math.cos(5 * math.pi / 8)
])
],
'5.25': [
np.array([
0.410, 1.900 + 0.175 * math.sin(7 * math.pi / 8),
-4.587 + 0.175 * math.cos(7 * math.pi / 8)
]),
np.array([
0.410, 2.388 + 0.175 * math.sin(7 * math.pi / 8),
-5.765 + 0.175 * math.cos(7 * math.pi / 8)
]),
np.array([
0.410, 2.761 + 0.175 * math.sin(7 * math.pi / 8),
-6.666 + 0.175 * math.cos(7 * math.pi / 8)
])
],
'6.75': [
np.array([
0.410, -1.986 + 0.175 * math.sin(-7 * math.pi / 8),
-4.795 + 0.175 * math.cos(-7 * math.pi / 8)
]),
np.array([
0.410, -2.445 + 0.175 * math.sin(-7 * math.pi / 8),
-5.904 + 0.175 * math.cos(-7 * math.pi / 8)
]),
np.array([
0.410, -2.790 + 0.175 * math.sin(-7 * math.pi / 8),
-6.735 + 0.175 * math.cos(-7 * math.pi / 8)
])
],
'8.25': [
np.array([
0.410, -4.726 + 0.175 * math.sin(-5 * math.pi / 8),
-1.957 + 0.175 * math.cos(-5 * math.pi / 8)
]),
np.array([
0.410, -5.696 + 0.175 * math.sin(-5 * math.pi / 8),
-2.359 + 0.175 * math.cos(-5 * math.pi / 8)
]),
np.array([
0.410, -6.596 + 0.175 * math.sin(-5 * math.pi / 8),
-2.732 + 0.175 * math.cos(-5 * math.pi / 8)
])
],
'9.75': [
np.array([
0.410, -5.044 + 0.175 * math.sin(-3 * math.pi / 8),
2.089 + 0.175 * math.cos(-3 * math.pi / 8)
]),
np.array([
0.410, -6.153 + 0.175 * math.sin(-3 * math.pi / 8),
2.549 + 0.175 * math.cos(-3 * math.pi / 8)
]),
np.array([
0.410, -6.984 + 0.175 * math.sin(-3 * math.pi / 8),
2.893 + 0.175 * math.cos(-3 * math.pi / 8)
])
],
'11.25': [
np.array([
0.410, -1.871 + 0.175 * math.sin(-math.pi / 8),
4.518 + 0.175 * math.cos(-math.pi / 8)
]),
np.array([
0.410, -2.417 + 0.175 * math.sin(-math.pi / 8),
5.834 + 0.175 * math.cos(-math.pi / 8)
]),
np.array([
0.410, -2.761 + 0.175 * math.sin(-math.pi / 8),
6.666 + 0.175 * math.cos(-math.pi / 8)
])
]
}
self.cutter_rotmat_dict = {
'0': [
np.eye(3),
rm.rotmat_from_euler(0, math.pi, 0),
np.eye(3),
rm.rotmat_from_euler(0, math.pi, 0),
np.eye(3),
np.eye(3),
np.eye(3),
np.eye(3),
np.eye(3),
np.eye(3)
],
'6': [
rm.rotmat_from_euler(0, math.pi, 0),
np.eye(3),
rm.rotmat_from_euler(0, math.pi, 0),
np.eye(3),
rm.rotmat_from_euler(0, math.pi, 0),
rm.rotmat_from_euler(0, math.pi, 0),
rm.rotmat_from_euler(0, math.pi, 0),
rm.rotmat_from_euler(0, math.pi, 0),
rm.rotmat_from_euler(0, math.pi, 0),
rm.rotmat_from_euler(0, math.pi, 0)
],
'3': [
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0)
],
'9': [
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(-math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, 0, 0),
rm.rotmat_from_euler(math.pi / 2, -math.pi / 6, 0, 'rxyz')
],
'1.5': [
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 4, 0, 0)
],
'4.5': [
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0),
rm.rotmat_from_euler(math.pi / 4, 0, 0)
],
'7.5': [
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, 0, 0),
rm.rotmat_from_euler(-math.pi / 4, math.pi / 4, 0, 'rxyz')
],
'10.5': [
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0)
],
'0.75': [
rm.rotmat_from_euler(7 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(7 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(7 * math.pi / 8, 0, 0)
],
'2.25': [
rm.rotmat_from_euler(5 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(5 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(5 * math.pi / 8, 0, 0)
],
'3.75': [
rm.rotmat_from_euler(3 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(3 * math.pi / 8, 0, 0)
],
'5.25': [
rm.rotmat_from_euler(math.pi / 8, 0, 0),
rm.rotmat_from_euler(math.pi / 8, 0, 0),
rm.rotmat_from_euler(math.pi / 8, 0, 0)
],
'6.75': [
rm.rotmat_from_euler(-math.pi / 8, 0, 0),
rm.rotmat_from_euler(-math.pi / 8, 0, 0),
rm.rotmat_from_euler(-math.pi / 8, 0, 0)
],
'8.25': [
rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0)
],
'9.75': [
rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0)
],
'11.25': [
rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0),
rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0)
]
}
self.cutters = {}
for k in self.cutter_pos_dict.keys():
self.cutters[k] = []
for i, pos in enumerate(self.cutter_pos_dict[k]):
tmp_jlc = jl.JLChain(pos=pos,
rotmat=self.cutter_rotmat_dict[k][i],
homeconf=np.zeros(1),
name='cutter_' + str(k) + '_' + str(i))
tmp_jlc.jnts[1]['loc_pos'] = np.zeros(3)
tmp_jlc.jnts[1]['loc_motionax'] = np.array([0, 0, 1])
tmp_jlc.lnks[1]['name'] = 'cutter'
tmp_jlc.lnks[1]['loc_pos'] = np.array([0, 0, 0])
tmp_jlc.lnks[1]['meshfile'] = os.path.join(
this_dir, 'meshes', 'cutter.stl')
tmp_jlc.lnks[1]['rgba'] = [1, 1, .0, 1.0]
tmp_jlc.tgtjnts = [1]
tmp_jlc.reinitialize()
self.cutters[k].append(tmp_jlc)
import numpy as np
import modeling.geometric_model as gm
import modeling.collision_model as cm
import visualization.panda.world as wd
import basis.robot_math as rm
import math
from scipy.spatial import cKDTree
import vision.depth_camera.surface.rbf_surface as rbfs
base = wd.World(cam_pos=np.array([-.3, -.7, .42]),
lookat_pos=np.array([0, 0, 0]))
# gm.gen_frame().attach_to(base)
bowl_model = cm.CollisionModel(initor="./objects/bowl.stl")
bowl_model.set_rgba([.3, .3, .3, .3])
bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0))
# bowl_model.attach_to(base)
pn_direction = np.array([0, 0, -1])
bowl_samples, bowl_sample_normals = bowl_model.sample_surface(
toggle_option='normals', radius=.002)
selection = bowl_sample_normals.dot(-pn_direction) > .1
bowl_samples = bowl_samples[selection]
bowl_sample_normals = bowl_sample_normals[selection]
tree = cKDTree(bowl_samples)
surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2])
surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base)
pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True)
tmp_direction = np.cross(pn_direction, pt_direction)
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
import os
tcp_loc_rotmat=None,
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()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='robotiq140', enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name)
this_dir, this_filename = os.path.split(__file__)
cpl_end_pos = self.coupling.jnts[-1]['gl_posq']
cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq']
# - lft_outer
self.lft_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='lft_outer')
self.lft_outer.jnts[1]['loc_pos'] = np.array([0, -.0306011, .054904])
self.lft_outer.jnts[1]['motion_rng'] = [.0, .7]
self.lft_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, 0)
self.lft_outer.jnts[1]['loc_motionax'] = np.array([-1, 0, 0])
self.lft_outer.jnts[2]['loc_pos'] = np.array([0, 0.01821998610742, 0.0260018192872234]) # passive
self.lft_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0])
self.lft_outer.jnts[3]['loc_pos'] = np.array([0, 0.0817554015893473, -0.0282203446692936])
self.lft_outer.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-0.725, 0, 0)
self.lft_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0])
self.lft_outer.jnts[4]['loc_pos'] = np.array([0, 0.0420203446692936, -.03242])
# - lft_inner
self.lft_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='lft_inner')
self.lft_inner.jnts[1]['loc_pos'] = np.array([0, -.0127, .06142])
self.lft_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, 0)
self.lft_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
# - rgt_outer
self.rgt_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='rgt_outer')
self.rgt_outer.jnts[1]['loc_pos'] = np.array([0, .0306011, .054904])
self.rgt_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, math.pi)
self.rgt_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[2]['loc_pos'] = np.array([0, 0.01821998610742, 0.0260018192872234]) # passive
self.rgt_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[3]['loc_pos'] = np.array([0, 0.0817554015893473, -0.0282203446692936])
self.rgt_outer.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-0.725, 0, 0)
self.rgt_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0])
self.rgt_outer.jnts[4]['loc_pos'] = np.array([0, 0.0420203446692936, -.03242])
# - rgt_inner
self.rgt_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='rgt_inner')
self.rgt_inner.jnts[1]['loc_pos'] = np.array([0, 0.0127, 0.06142])
self.rgt_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, math.pi)
self.rgt_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
# links
# - lft_outer
self.lft_outer.lnks[0]['name'] = "robotiq140_gripper_base"
self.lft_outer.lnks[0]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[0]['com'] = np.array([8.625e-08, -4.6583e-06, 0.03145])
self.lft_outer.lnks[0]['mass'] = 0.22652
self.lft_outer.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_base_link.stl")
self.lft_outer.lnks[0]['rgba'] = [.2, .2, .2, 1]
self.lft_outer.lnks[1]['name'] = "left_outer_knuckle"
self.lft_outer.lnks[1]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[1]['com'] = np.array([-0.000200000000003065, 0.0199435877845359, 0.0292245259211331])
self.lft_outer.lnks[1]['mass'] = 0.00853198276973456
self.lft_outer.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_knuckle.stl")
self.lft_outer.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
self.lft_outer.lnks[2]['name'] = "left_outer_finger"
self.lft_outer.lnks[2]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[2]['com'] = np.array([0.00030115855001899, 0.0373907951953854, -0.0208027427000385])
self.lft_outer.lnks[2]['mass'] = 0.022614240507152
self.lft_outer.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_finger.stl")
self.lft_outer.lnks[2]['rgba'] = [.2, .2, .2, 1]
self.lft_outer.lnks[3]['name'] = "left_inner_finger"
self.lft_outer.lnks[3]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[3]['com'] = np.array([0.000299999999999317, 0.0160078233491243, -0.0136945669206257])
self.lft_outer.lnks[3]['mass'] = 0.0104003125914103
self.lft_outer.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_finger.stl")
self.lft_outer.lnks[3]['rgba'] = [.2, .2, .2, 1]
self.lft_outer.lnks[4]['name'] = "left_inner_finger_pad"
self.lft_outer.lnks[4]['loc_pos'] = np.zeros(3)
self.lft_outer.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_pad.stl")
self.lft_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3]
self.lft_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
# - lft_inner
self.lft_inner.lnks[1]['name'] = "left_inner_knuckle"
self.lft_inner.lnks[1]['loc_pos'] = np.zeros(3)
self.lft_inner.lnks[1]['com'] = np.array([0.000123011831763771, 0.0507850843201817, 0.00103968640075166])
self.lft_inner.lnks[1]['mass'] = 0.0271177346495152
self.lft_inner.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_knuckle.stl")
self.lft_inner.lnks[1]['rgba'] = [.2, .2, .2, 1]
# - rgt_outer
self.rgt_outer.lnks[1]['name'] = "right_outer_knuckle"
self.rgt_outer.lnks[1]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[1]['com'] = np.array([-0.000200000000003065, 0.0199435877845359, 0.0292245259211331])
self.rgt_outer.lnks[1]['mass'] = 0.00853198276973456
self.rgt_outer.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_knuckle.stl")
self.rgt_outer.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
self.rgt_outer.lnks[2]['name'] = "right_outer_finger"
self.rgt_outer.lnks[2]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[2]['com'] = np.array([0.00030115855001899, 0.0373907951953854, -0.0208027427000385])
self.rgt_outer.lnks[2]['mass'] = 0.022614240507152
self.rgt_outer.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_finger.stl")
self.rgt_outer.lnks[2]['rgba'] = [.2, .2, .2, 1]
self.rgt_outer.lnks[3]['name'] = "right_inner_finger"
self.rgt_outer.lnks[3]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[3]['com'] = np.array([0.000299999999999317, 0.0160078233491243, -0.0136945669206257])
self.rgt_outer.lnks[3]['mass'] = 0.0104003125914103
self.rgt_outer.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_finger.stl")
self.rgt_outer.lnks[3]['rgba'] = [.2, .2, .2, 1]
self.rgt_outer.lnks[4]['name'] = "right_inner_finger_pad"
self.rgt_outer.lnks[4]['loc_pos'] = np.zeros(3)
self.rgt_outer.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_pad.stl")
self.rgt_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3]
self.rgt_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1]
# - rgt_inner
self.rgt_inner.lnks[1]['name'] = "right_inner_knuckle"
self.rgt_inner.lnks[1]['loc_pos'] = np.zeros(3)
self.rgt_inner.lnks[1]['com'] = np.array([0.000123011831763771, 0.0507850843201817, 0.00103968640075166])
self.rgt_inner.lnks[1]['mass'] = 0.0271177346495152
self.rgt_inner.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_knuckle.stl")
self.rgt_inner.lnks[1]['rgba'] = [.2, .2, .2, 1]
# reinitialize
self.lft_outer.reinitialize()
self.lft_inner.reinitialize()
self.rgt_outer.reinitialize()
self.rgt_inner.reinitialize()
# jaw width
self.jawwidth_rng = [0.0, .140]
# jaw center
self.jaw_center_pos = np.array([0, 0, .19]) # position for initial state (fully open)
# relative jaw center pos
self.jaw_center_pos_rel = self.jaw_center_pos - self.lft_outer.jnts[4]['gl_posq']
# collision detection
self.all_cdelements = []
self.enable_cc(toggle_cdprimit=enable_cc)
gm.gen_frame().attach_to(base)
xav = XArmShuidi(enable_cc=True)
xav.fk(component_name='all',
jnt_values=np.array(
[0, 0, 0, 0, 0, 0, math.pi, 0, -math.pi / 6, 0, 0]))
xav.jaw_to(jawwidth=.08)
tgt_pos = np.array([.55, 0, .55])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
jnt_values = xav.ik(component_name='arm',
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat)
print(jnt_values)
tgt_pos2 = np.array([.45, 0, .07])
tgt_rotmat2 = rm.rotmat_from_euler(0, math.pi, 0)
jnt_values2 = xav.ik(component_name='arm',
tgt_pos=tgt_pos2,
tgt_rotmat=tgt_rotmat2,
seed_jnt_values=jnt_values,
max_niter=10000)
print(jnt_values2)
xav.fk(component_name='arm', jnt_values=jnt_values)
# xss.fk(component_name='agv', jnt_values=np.array([.2, -.5, math.radians(30)]))
xav_meshmodel = xav.gen_meshmodel(toggle_tcpcs=True)
xav_meshmodel.attach_to(base)
# xss.show_cdprimit()
xav.gen_stickmodel().attach_to(base)
tic = time.time()
result = xav.is_collided()
toc = time.time()
import robot_sim.robots.nextage.nextage as nxt
import motion.probabilistic.rrt_connect as rrtc
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_waist'
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_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_conf = robot_s.ik(component_name, goal_pos, goal_rotmat)
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=.05,
smoothing_iterations=150,
max_time=300)
print(path)
for pose in path[1:-2]:
print(pose)
component_name,
gl_jaw_center_pos,
gl_jaw_center_rotmat,
start_conf=robot_s.get_jnt_values(component_name),
approach_direction=gl_jaw_center_rotmat[:, 2],
approach_distance=.2)
if conf_path is None:
continue
else:
robot_s.fk(component_name, conf_path[-1])
rel_obj_pos, rel_obj_rotmat = robot_s.hold(object_box,
jawwidth=jaw_width)
# robot_s.gen_meshmodel().attach_to(base)
# base.run()
goal_jaw_center_pos = gl_jaw_center_pos + np.array([-.3, -.2, .3])
goal_jaw_center_rotmat = rm.rotmat_from_euler(math.pi / 2, math.pi / 2,
math.pi / 2)
jvs = robot_s.ik(component_name, goal_jaw_center_pos,
goal_jaw_center_rotmat)
robot_s.fk(component_name, jvs)
# robot_s.gen_meshmodel().attach_to(base)
# base.run()
path = rrtc_s.plan(component_name,
start_conf=conf_path[-1],
goal_conf=jvs,
obstacle_list=[],
ext_dist=.1,
rand_rate=70,
max_time=300)
for jvp in path:
robot_s.fk(component_name, jvp)
gl_obj_pos, gl_obj_rotmat = robot_s.cvt_loc_tcp_to_gl(
rel_pos = np.zeros(3)
rel_rotmat = np.eye(3)
if pressed_keys['r']:
rel_pos = np.array([arm_linear_speed * .5, 0, 0])
elif pressed_keys['t']:
rel_pos = np.array([-arm_linear_speed * .5, 0, 0])
elif pressed_keys['f']:
rel_pos = np.array([0, arm_linear_speed * .5, 0])
elif pressed_keys['g']:
rel_pos = np.array([0, -arm_linear_speed * .5, 0])
elif pressed_keys['v']:
rel_pos = np.array([0, 0, arm_linear_speed * .5])
elif pressed_keys['b']:
rel_pos = np.array([0, 0, -arm_linear_speed * .5])
elif pressed_keys['y']:
rel_rotmat = rm.rotmat_from_euler(arm_angular_speed * .5, 0, 0)
elif pressed_keys['u']:
rel_rotmat = rm.rotmat_from_euler(-arm_angular_speed * .5, 0, 0)
elif pressed_keys['h']:
rel_rotmat = rm.rotmat_from_euler(0, arm_angular_speed * .5, 0)
elif pressed_keys['j']:
rel_rotmat = rm.rotmat_from_euler(0, -arm_angular_speed * .5, 0)
elif pressed_keys['n']:
rel_rotmat = rm.rotmat_from_euler(0, 0, arm_angular_speed * .5)
elif pressed_keys['m']:
rel_rotmat = rm.rotmat_from_euler(0, 0, -arm_angular_speed * .5)
new_arm_tcp_pos = current_arm_tcp_pos + rel_pos
new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat)
last_jnt_values = rbt_s.get_jnt_values()
new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos,
tgt_rotmat=new_arm_tcp_rotmat,
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
homeconf=np.zeros(7),
name='sia5',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
self.jlc = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=homeconf,
name=name)
# seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1
jnt_safemargin = math.pi / 18.0
self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0])
self.jlc.jnts[1]['motion_rng'] = [-math.pi + jnt_safemargin, math.pi]
self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, .168])
self.jlc.jnts[2]['motion_rng'] = [
-math.radians(110) + jnt_safemargin,
math.radians(110) - jnt_safemargin
]
self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0])
self.jlc.jnts[3]['loc_pos'] = np.array([0, 0, .27])
self.jlc.jnts[3]['motion_rng'] = [
-math.radians(170) + jnt_safemargin,
math.radians(170) - jnt_safemargin
]
self.jlc.jnts[4]['loc_pos'] = np.array([.085, 0, 0])
self.jlc.jnts[4]['motion_rng'] = [
-math.radians(90) + jnt_safemargin,
math.radians(115) - jnt_safemargin
]
self.jlc.jnts[4]['loc_motionax'] = np.array([0, -1, 0])
self.jlc.jnts[5]['loc_pos'] = np.array([.27, -0, .06])
self.jlc.jnts[5]['motion_rng'] = [
-math.radians(90) + jnt_safemargin,
math.radians(90) - jnt_safemargin
]
self.jlc.jnts[5]['loc_motionax'] = np.array([-1, 0, 0])
self.jlc.jnts[6]['loc_pos'] = np.array([0, 0, 0])
self.jlc.jnts[6]['motion_rng'] = [
-math.radians(110) + jnt_safemargin,
math.radians(110) - jnt_safemargin
]
self.jlc.jnts[6]['loc_motionax'] = np.array([0, -1, 0])
self.jlc.jnts[7]['loc_pos'] = np.array([.134, .0, .0])
self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2, 0, 0)
self.jlc.jnts[7]['motion_rng'] = [
-math.radians(90) + jnt_safemargin,
math.radians(90) - jnt_safemargin
]
self.jlc.jnts[7]['loc_motionax'] = np.array([-1, 0, 0])
self.jlc.jnts[8]['loc_pos'] = np.array([.011, .0, .0])
self.jlc.jnts[8]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2, -math.pi / 2, -math.pi / 2)
# links
# self.jlc.lnks[0]['name'] = "base"
# self.jlc.lnks[0]['loc_pos'] = np.zeros(3)
# self.jlc.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base.dae")
# self.jlc.lnks[0]['rgba'] = [.5,.5,.5, 1.0]
self.jlc.lnks[1]['name'] = "link_s"
self.jlc.lnks[1]['loc_pos'] = np.array([0, 0, .142])
self.jlc.lnks[1]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi)
self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes",
"link_s.dae")
self.jlc.lnks[1]['rgba'] = [.55, .55, .55, 1.0]
self.jlc.lnks[2]['name'] = "link_l"
self.jlc.lnks[2]['loc_pos'] = np.zeros(3)
self.jlc.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi)
self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes",
"link_l.dae")
self.jlc.lnks[2]['rgba'] = [.1, .3, .5, 1.0]
self.jlc.lnks[3]['name'] = "link_e"
self.jlc.lnks[3]['loc_pos'] = np.zeros(3)
self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes",
"link_e.dae")
self.jlc.lnks[3]['rgba'] = [.7, .7, .7, 1.0]
self.jlc.lnks[4]['name'] = "link_u"
self.jlc.lnks[4]['loc_pos'] = np.zeros(3)
self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes",
"link_u.dae")
self.jlc.lnks[4]['rgba'] = [.1, .3, .5, 1.0]
self.jlc.lnks[5]['name'] = "link_r"
self.jlc.lnks[5]['loc_pos'] = np.zeros(3)
self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes",
"link_r.dae")
self.jlc.lnks[5]['rgba'] = [.7, .7, .7, 1.0]
self.jlc.lnks[6]['name'] = "link_b"
self.jlc.lnks[6]['loc_pos'] = np.zeros(3)
self.jlc.lnks[6]['meshfile'] = os.path.join(this_dir, "meshes",
"link_b.dae")
self.jlc.lnks[6]['rgba'] = [.1, .3, .5, 1.0]
self.jlc.lnks[7]['name'] = "link_t"
self.jlc.lnks[7]['loc_pos'] = np.zeros(3)
self.jlc.lnks[7]['meshfile'] = os.path.join(this_dir, "meshes",
"link_t.dae")
self.jlc.lnks[7]['rgba'] = [.7, .7, .7, 1.0]
# reinitialization
# self.jlc.setinitvalues(np.array([-math.pi/2, math.pi/3, math.pi/6, 0, 0, 0, 0]))
# self.jlc.setinitvalues(np.array([-math.pi/2, 0, math.pi/3, math.pi/10, 0, 0, 0]))
self.jlc.reinitialize()
# collision detection
if enable_cc:
self.enable_cc()
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
homeconf=np.zeros(6),
name='ur3',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
self.jlc = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=homeconf,
name=name)
# six joints, n_jnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1
self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, .1519])
self.jlc.jnts[2]['loc_pos'] = np.array([0, 0.1198, 0])
self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(
.0, math.pi / 2.0, .0)
self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0])
self.jlc.jnts[3]['loc_pos'] = np.array([0, -.0925, .24365])
self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(.0, .0, .0)
self.jlc.jnts[3]['loc_motionax'] = np.array([0, 1, 0])
self.jlc.jnts[4]['loc_pos'] = np.array([.0, .0, 0.21325])
self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler(
.0, math.pi / 2.0, 0)
self.jlc.jnts[4]['loc_motionax'] = np.array([0, 1, 0])
self.jlc.jnts[5]['loc_pos'] = np.array([0, .11235 + .0925 - .1198, 0])
self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0)
self.jlc.jnts[5]['loc_motionax'] = np.array([0, 0, 1])
self.jlc.jnts[6]['loc_pos'] = np.array([0, 0, .08535])
self.jlc.jnts[6]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0)
self.jlc.jnts[6]['loc_motionax'] = np.array([0, 1, 0])
self.jlc.jnts[7]['loc_pos'] = np.array([0, .0819, 0])
self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2.0, math.pi / 2.0, 0)
# self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0)
# links
self.jlc.lnks[0]['name'] = "base"
self.jlc.lnks[0]['loc_pos'] = np.zeros(3)
self.jlc.lnks[0]['mass'] = 2.0
self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes",
"base.stl")
self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0]
self.jlc.lnks[1]['name'] = "shoulder"
self.jlc.lnks[1]['loc_pos'] = np.zeros(3)
self.jlc.lnks[1]['com'] = np.array([.0, -.02, .0])
self.jlc.lnks[1]['mass'] = 1.95
self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes",
"shoulder.stl")
self.jlc.lnks[1]['rgba'] = [.1, .3, .5, 1.0]
self.jlc.lnks[2]['name'] = "upperarm"
self.jlc.lnks[2]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[2]['com'] = np.array([.13, 0, .1157])
self.jlc.lnks[2]['mass'] = 3.42
self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes",
"upperarm.stl")
# self.jlc.lnks[2]['rgba'] = [.7,.7,.7, 1.0]
self.jlc.lnks[3]['name'] = "forearm"
self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[3]['com'] = np.array([.05, .0, .0238])
self.jlc.lnks[3]['mass'] = 1.437
self.jlc.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes",
"forearm.stl")
self.jlc.lnks[3]['rgba'] = [.35, .35, .35, 1.0]
self.jlc.lnks[4]['name'] = "wrist1"
self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[4]['com'] = np.array([.0, .0, 0.01])
self.jlc.lnks[4]['mass'] = 0.871
self.jlc.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes",
"wrist1.stl")
# self.jlc.lnks[4]['rgba'] = [.7,.7,.7, 1.0]
self.jlc.lnks[5]['name'] = "wrist2"
self.jlc.lnks[5]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[5]['com'] = np.array([.0, .0, 0.01])
self.jlc.lnks[5]['mass'] = 0.8
self.jlc.lnks[5]['mesh_file'] = os.path.join(this_dir, "meshes",
"wrist2.stl")
self.jlc.lnks[5]['rgba'] = [.1, .3, .5, 1.0]
self.jlc.lnks[6]['name'] = "wrist3"
self.jlc.lnks[6]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[6]['com'] = np.array([.0, .0, -0.02])
self.jlc.lnks[6]['mass'] = 0.8
self.jlc.lnks[6]['mesh_file'] = os.path.join(this_dir, "meshes",
"wrist3.stl")
self.jlc.lnks[6]['rgba'] = [.5, .5, .5, 1.0]
self.jlc.reinitialize()
# collision detection
if enable_cc:
self.enable_cc()
