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 gettwoend(self):
self.endpairs=[]
self.temporigin = []
self.origin=[]
self.homomat=[]
for i, coordinate in enumerate(self.coordinate):
endpair = []
axis = coordinate.T[2]
large = 0
large_vec = np.array([0,0,0])
small = 0
small_vec = np.array([0, 0, 0])
facet_1_ID=self.doubleholdpair[i][0]
facet_2_ID = self.doubleholdpair[i][1]
vertices = np.append(self.largeface_vertices[facet_1_ID], self.largeface_vertices[facet_2_ID], axis = 0)
for vertice in vertices:
value = np.dot(axis, vertice)
if value>=large:
large = value
large_vec = vertice
elif value<small:
small = value
small_vec = vertice
endpair.append(large_vec)
endpair.append(small_vec)
self.temporigin.append((large_vec+small_vec)*0.5)
self.endpairs.append(endpair)
origin = self.getorigin(normal_list=[self.largeface_normals[facet_1_ID],self.largeface_normals[facet_2_ID], axis], point_list=[self.largefacescenter[facet_1_ID],self.largefacescenter[facet_2_ID],(large_vec+small_vec)*0.5])
self.origin.append(origin)
homomat = np.linalg.inv(rm.homomat_from_posrot(origin, coordinate))
self.pRotMat.append(homomat)
self.pCoM.append(rm.homomat_transform_points(homomat, self.com))
tempvertice = []
for vertice in vertices:
tempvertice.append(rm.homomat_transform_points(homomat, vertice))
self.pVertices.append(tempvertice)
self.pNormals.append([np.dot(coordinate.T, self.largeface_normals[facet_1_ID]),np.dot(coordinate.T, self.largeface_normals[facet_2_ID])])
self.pFacetpairscenter.append([rm.homomat_transform_points(homomat,self.largefacescenter[facet_1_ID]), rm.homomat_transform_points(homomat,self.largefacescenter[facet_2_ID])])
def gen_roundstick(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=0.005,
count=[8, 8]):
"""
:param spos:
:param epos:
:param thickness:
:return: a Trimesh object (Primitive)
author: weiwei
date: 20191228osaka
"""
pos = spos
height = np.linalg.norm(epos - spos)
if np.allclose(height, 0):
rotmat = np.eye(3)
else:
rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos)
homomat = rm.homomat_from_posrot(pos, rotmat)
return tp.Capsule(height=height,
radius=thickness / 2.0,
count=count,
homomat=homomat)
def gen_meshmodel(self,
tcp_jntid=None,
tcp_loc_pos=None,
tcp_loc_rotmat=None,
toggle_tcpcs=True,
toggle_jntscs=False,
name='robot_mesh',
rgba=None):
mm_collection = mc.ModelCollection(name=name)
for id in range(self.jlobject.ndof + 1):
if self.jlobject.lnks[id]['collisionmodel'] is not None:
this_collisionmodel = self.jlobject.lnks[id][
'collisionmodel'].copy()
pos = self.jlobject.lnks[id]['gl_pos']
rotmat = self.jlobject.lnks[id]['gl_rotmat']
this_collisionmodel.set_homomat(
rm.homomat_from_posrot(pos, rotmat))
this_rgba = self.jlobject.lnks[id][
'rgba'] if rgba is None else rgba
this_collisionmodel.set_rgba(this_rgba)
this_collisionmodel.attach_to(mm_collection)
# tool center coord
if toggle_tcpcs:
self._toggle_tcpcs(mm_collection,
tcp_jntid,
tcp_loc_pos,
tcp_loc_rotmat,
tcpic_rgba=np.array([.5, 0, 1, 1]),
tcpic_thickness=.0062)
# toggle all coord
if toggle_jntscs:
alpha = 1 if rgba == None else rgba[3]
self._toggle_jntcs(mm_collection,
jntcs_thickness=.0062,
alpha=alpha)
return mm_collection
def gen_rectstick(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=.005,
sections=8):
"""
:param spos: 1x3 nparray
:param epos: 1x3 nparray
:param thickness: 0.005 m by default
:param sections: # of discretized sectors used to approximate a cylinder
:return: a Trimesh object (Primitive)
author: weiwei
date: 20191228osaka
"""
pos = spos
height = np.linalg.norm(epos - spos)
if np.allclose(height, 0):
rotmat = np.eye(3)
else:
rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos)
homomat = rm.homomat_from_posrot(pos, rotmat)
return tp.Cylinder(height=height,
radius=thickness / 2.0,
sections=sections,
homomat=homomat)
# planecm.setMat(base.pg.np4ToMat4(planenode.gethomomat()))
# print(planenode.gethomomat())
# Boxes
# model = loader.loadModel('models/box.egg')
model = cm.CollisionModel("./objects/bunnysim.stl")
node = bbd.BDBody(model, cdtype='box', dynamic=True)
bulletnodelist = []
for i in range(10):
# node = bch.genBulletCDMesh(model)
# newnode = copy.deepcopy(node)
newnode = node.copy()
newnode.setMass(1)
rot = rm.rotmat_from_axangle([0, 1, 0], -math.pi / 4)
pos = np.array([0, 0, .1 + i * .3])
newnode.set_homomat(rm.homomat_from_posrot(pos, rot))
base.physicsworld.attachRigidBody(newnode)
bulletnodelist.append(newnode)
# modelcopy = copy.deepcopy(model)
# modelcopy.setColor(.8, .6, .3, .5)
# modelcopy.reparentTo(np)
# debugNode = BulletDebugNode('Debug')
# debugNode.showWireframe(True)
# debugNode.showConstraints(True)
# debugNode.showBoundingBoxes(False)
# debugNode.showNormals(False)
# debugNP = base.render.attachNewNode(debugNode)
# # debugNP.show()
# world.setDebugNode(debugNP.node())
restitution = 0.5
heightlist = [0.001 * i for i in range(820, 860, 30)]
errorrange_xyz = 0.003
errorrange_rpy = np.radians(2)
errorconfiguration = geterrorconfiguration(errorrange_xyz, errorrange_rpy)
xyz_error_print = printonscreen(pos=(-1, 0.7, 0), words="Error range on xyz: " + str(errorrange_xyz) + " (mm)")
rpy_error_print = printonscreen(pos=(-1, 0.6, 0), words="Error range on rpy: " + str(errorrange_rpy) + " (degree)")
# obj_box = cm.gen_box(extent=[.2, 1, .3], rgba=[.3, 0, 0, 1])
obj_box = cm.gen_sphere(radius=.5, rgba=[.3, 0, 0, 1])
obj_bd_box = bdm.BDModel(obj_box, mass=.3, type="triangles")
obj_bd_box.set_pos(np.array([.7, 0, 2.7]))
obj_bd_box.start_physics()
base.attach_internal_update_obj(obj_bd_box)
plane1 = cm.gen_box([0.30000, 0.3000, .3000100], rm.homomat_from_posrot([.500, 0, -.050]))
plane1bm = bdm.BDModel(objinit=plane1, mass=1)
plane1bm.set_rgba((.5, .5, .5, 1))
plane1bm.set_pos(np.array([0, 0, 1.000]))
# plane1bm.start_physics()
# base.attach_internal_update_obj(plane1bm)
plane = cm.gen_box([5.0000, 5.000, .000100], rm.homomat_from_posrot([.500, 0, -.050]))
planebm = bdm.BDModel(objinit=plane, mass=0)
planebm.set_rgba((.5, .5, .5, 1))
planebm.set_pos(np.array([0, 0, 0.600]))
planebm.start_physics()
base.attach_internal_update_obj(planebm)
address = this_dir + "/PlacementData"
# objname = "test_long"
def getplacementRotMat(self):
self.p = []
for i in range(len(self.rotmat)):
for j in range(len(self.rotmat[i])):
com = copy.deepcopy(self.com)
holdingpairsnormals = self.hpairsnormals[i]
holdingpairscenters = self.hpairscenters[i]
temholdingpairsvertices = self.hpairsvertices[i]
# sloperot = None
a = self.rotmat[i][j]
b = self.holdingpairsSptPnt[i]
addvector = np.array([[1], [1], [1]])
holdingpairscenters = np.concatenate(
(holdingpairscenters, addvector), axis=1)
addvector_vertices = np.array([1])
addvector_com = np.array([1])
print(com)
com = np.concatenate((np.array(com), addvector_com), axis=0)
holdingpairsvertices = []
for temvertices in temholdingpairsvertices:
tempverticeonSFC = []
for vertices in temvertices:
vertices = np.concatenate(
(vertices, addvector_vertices), axis=0)
tempverticeonSFC.append(vertices)
holdingpairsvertices.append(tempverticeonSFC)
sloperot = rm.homomat_from_posrot(rot=np.dot(
rm.rotmat_from_axangle([0, 1, 0], np.radians(-54.74)),
rm.rotmat_from_axangle([0, 0, 1], np.radians(-45))))
# sloperot = np.eye(4)
# sloperot2 = da.npmat4_to_pdmat4(sloperot1)
# sloperot3 = da.pdmat4_to_npmat4(sloperot2)
p = da.npmat4_to_pdmat4(rm.homomat_from_posrot(b, a))
self.p.append(p)
p = da.pdmat4_to_npmat4(p)
p = np.linalg.inv(p)
p = np.dot(sloperot, p)
placementNormals0 = np.dot(p[0:3, 0:3],
holdingpairsnormals[0].T).T
placementNormals1 = np.dot(p[0:3, 0:3],
holdingpairsnormals[1].T).T
placementNormals2 = np.dot(p[0:3, 0:3],
holdingpairsnormals[2].T).T
placementfacetpairscenter0 = np.dot(p,
holdingpairscenters[0].T).T
placementfacetpairscenter1 = np.dot(p,
holdingpairscenters[1].T).T
placementfacetpairscenter2 = np.dot(p,
holdingpairscenters[2].T).T
#change CoM
# x_e=5
# y_e = 5
# z_e = 5
# CoMRotMat = np.array([[1,0,0,x_e],[0,1,0,y_e],[0,0,1,z_e],[0,0,0,1]])
# com = np.dot(CoMRotMat,com.T)
#-----------------------
print(com.T)
placementCom0 = np.dot(p, com.T).T
dtemplacementVertices = []
for vertices in holdingpairsvertices:
templacementVertices = []
for vertice in vertices:
placementfacetpairvertices = np.dot(p, vertice.T).T
templacementVertices.append(placementfacetpairvertices)
dtemplacementVertices.append(templacementVertices)
ddtemplacementface = []
for placement in self.hpairsfaces_triple[i][j]:
dtemplacementface = []
for face in placement:
templacementface = []
for faceverticeID in face:
faceverticePos = np.concatenate(
(self.vertices[faceverticeID], np.array([1])),
axis=0)
rotatedfaceveritce = np.dot(p, faceverticePos.T).T
templacementface.append(
rotatedfaceveritce) #every small face
dtemplacementface.append(
templacementface) #every facet
ddtemplacementface.append(
dtemplacementface) #every placement
p = da.npmat4_to_pdmat4(p)
self.pRotMat.append(p)
self.pNormals.append([
placementNormals0[0:3], placementNormals1[0:3],
placementNormals2[0:3]
])
self.pFacetpairscenter.append([
placementfacetpairscenter0[0:3],
placementfacetpairscenter1[0:3],
placementfacetpairscenter2[0:3]
])
self.pVertices.append(dtemplacementVertices)
self.pCoM.append(placementCom0)
self.pFaces.append(ddtemplacementface)
# elif x ==13:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008])
# elif x ==14:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x ==15:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011])
# elif x ==16:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x == 17:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008])
# elif x == 18:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005])
# elif x == 19:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001])
c1 = cm.gen_box(extent=[.006*18.5, 0.006*9, .001], homomat=rm.homomat_from_posrot([0.006*18,0.006*6.5,0], rm.rotmat_from_axangle([0,0,1], 0*np.pi/2)),rgba=[0,0,0,0.2])
c2 = cm.gen_box(extent=[.006*18.5, 0.006*12, .001], homomat=rm.homomat_from_posrot([0.006*18,0.006*29,0], rm.rotmat_from_axangle([0,0,1], 0*np.pi/2)),rgba=[0, 0, 0, 0.2])
c3 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0, 0.02, 0], rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c4 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0.02,0,0], rm.rotmat_from_axangle([0,0,1], 0)), rgba=[0, 0, 0, 0.2])
# c1 = cm.gen_box(extent=[.006 * 20, 0.006 * 30, .001], homomat=rm.homomat_from_posrot([0.006 * 19, 0.006 * 4.5, 0],
# rm.rotmat_from_axangle(
# [0, 0, 1],
# 0 * np.pi / 2)),
# rgba=[0, 0, 0, 0.2])
# c2 = cm.gen_box(extent=[.006 * 3, 0.006 * 3, .001], homomat=rm.homomat_from_posrot([0.006 * 4, 0.006 * 12, 0],
# rm.rotmat_from_axangle(
# [0, 0, 1],
# 0 * np.pi / 2)),
# rgba=[0, 0, 0, 0.2])
import numpy as np
import basis.robot_math as rm
base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
gm.gen_frame().attach_to(base)
objcm = cm.CollisionModel('tubebig.stl')
robot_s = ym.Yumi(enable_cc=True)
manipulator_name = 'rgt_arm'
hand_name = 'rgt_hnd'
start_conf = robot_s.get_jnt_values(manipulator_name)
goal_homomat_list = []
for i in range(6):
goal_pos = np.array([.55, -.1, .3]) - np.array([i * .1, i * .1, 0])
# goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_rotmat = np.eye(3)
goal_homomat_list.append(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm = objcm.copy()
tmp_objcm.set_rgba([1, 0, 0, .3])
tmp_objcm.set_homomat(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm.attach_to(base)
grasp_info_list = gutil.load_pickle_file(objcm_name='tubebig', file_name='yumi_tube_big.pickle')
grasp_info = grasp_info_list[0]
pp_planner = PickPlacePlanner(robot_s=robot_s)
conf_list, jawwidth_list, objpose_list = \
pp_planner.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=objcm,
grasp_info_list=grasp_info_list,
goal_homomat_list=goal_homomat_list,
start_conf=robot_s.get_jnt_values(hand_name),
end_conf=robot_s.get_jnt_values(hand_name),
depart_direction_list=[np.array([0, 0, 1])] * len(goal_homomat_list),
import motion.probabilistic.rrt_connect as rrtc
import manipulation.pick_place_planner as ppp
if __name__ == '__main__':
# world
base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
object = cm.CollisionModel("./objects/tubebig.stl")
object.set_rgba([.5, .7, .3, 1])
gm.gen_frame().attach_to(object)
object_start = object.copy()
object_start_pos = np.array([0.800, -.350, 0.900])
object_start_rotmat = np.eye(3)
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)
object_goal = object.copy()
object_goal_pos = np.array([0.800, -.300, 0.900])
object_goal_rotmat = np.eye(3)
object_goal_homomat = rm.homomat_from_posrot(object_goal_pos,
object_goal_rotmat)
object_goal.set_pos(object_goal_pos)
object_goal.set_rotmat(object_goal_rotmat)
object_goal.attach_to(base)
# robot_instance = ur3d.UR3Dual()
robot = ur3ed.UR3EDual()
]
for i in obstacle_list:
i.show_cdprimit()
# base.run()
## show tool
tool = cm.gen_box(extent=[.05, .2, .05])
tool_initial = tool.copy()
tool_final = tool.copy()
tool_initial.set_rgba([0, 0, 1, 1])
tool_initial_pos = tool_table_pos + np.array([0, 0, .15 + .03])
tool_initial_rotmat = rm.rotmat_from_axangle([0, 0, 1], math.radians(90))
tool_initial.set_pos(tool_initial_pos)
tool_initial.set_rotmat(tool_initial_rotmat)
# tool_initial.attach_to(base)
tool_initial.show_localframe()
tool_initial_homomat = rm.homomat_from_posrot(tool_initial_pos, rotmat)
tool_final.set_rgba([0, 0, 1, 1])
tool_final_pos = np.array([
table2_center[0], table2_center[1],
object_box4.get_pos()[2] + .009 + .11
])
rotmat = np.dot(rm.rotmat_from_axangle([0, 1, 0], math.radians(90)),
rm.rotmat_from_axangle([0, 0, 1], math.radians(90)))
tool_final.set_pos(tool_final_pos)
tool_final.set_rotmat(rotmat)
tool_final_homomat = rm.homomat_from_posrot(tool_final_pos, rotmat)
## rbt_s and grasp list
robot_s = xsm.XArm7YunjiMobile()
rrtc_s = rrtc.RRTConnect(robot_s)
rrtc_planner = rrtdwc.RRTDWConnect(robot_s)
grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
# elif x ==14:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x ==15:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011])
# elif x ==16:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x == 17:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008])
# elif x == 18:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005])
# elif x == 19:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001])
#left right
c1 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 8, .001],
homomat=rm.homomat_from_posrot(
[0.006 * 22, 0.006 * 9, 0],
rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c2 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 8, .001],
homomat=rm.homomat_from_posrot(
[0.006 * 22, 0.006 * 29, 0],
rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)),
rgba=[0, 0, 0, 0.2])
#top down
c3 = cm.gen_box(extent=[.006 * 4, 0.006 * 28.5, .001],
homomat=rm.homomat_from_posrot(
[0.006 * 3.5, 0.006 * 19, 0],
rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c4 = cm.gen_box(extent=[.006 * 4, 0.006 * 28.5, .001],
homomat=rm.homomat_from_posrot(
# elif x ==14:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x ==15:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011])
# elif x ==16:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010])
# elif x == 17:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008])
# elif x == 18:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005])
# elif x == 19:
# matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001])
c1 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 9, .001],
homomat=rm.homomat_from_posrot(
[0.006 * 18, 0.006 * 6.5, 0],
rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c2 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 12, .001],
homomat=rm.homomat_from_posrot(
[0.006 * 18, 0.006 * 29, 0],
rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c3 = cm.gen_box(extent=[.010, .050, .001],
homomat=rm.homomat_from_posrot([0, 0.02, 0],
rm.rotmat_from_axangle(
[0, 0, 1], np.pi / 2)),
rgba=[0, 0, 0, 0.2])
c4 = cm.gen_box(extent=[.010, .050, .001],
homomat=rm.homomat_from_posrot([0.02, 0, 0],
rm.rotmat_from_axangle(
testobj = bdm.BDModel(objpath,
mass=1,
restitution=restitution,
dynamic=True,
friction=friction,
type="triangles")
# testobj.set_scale((0.001,0.001,0.001))
testobj.set_rgba((.3, .5, .7, 0.5))
testobj.start_physics()
testobj.set_linearDamping(0.1)
testobj.set_angularDamping(0.1)
objbm.append(testobj)
# table
plane = cm.gen_box([5.0000, 5.000, .000100],
rm.homomat_from_posrot([.500, 0, -.050]))
planebm = bdm.BDModel(objinit=plane, mass=0, friction=friction)
planebm.set_rgba((.5, .5, .5, 1))
planebm.set_pos(np.array([0, 0, 0.600]))
planebm.start_physics()
# slope
slope = slope.Slope(z=0, placement="ping", size=0.5)
slopemodels = slope.getSlopeDym(mass=0,
restitution=restitution,
dynamic=True,
friction=friction)
slopePos = [0.600, 0, 0.78]
slopemodels[0].set_pos(slopePos)
slopemodels[1].set_pos(slopePos)
slopemodels[2].set_pos(slopePos)
# object_goal = object.copy()
# object_goal_pos = np.array([0.800, -.300, 0.900])
# object_goal_rotmat = np.eye(3)
# object_goal_homomat = rm.homomat_from_posrot(object_goal_pos, object_goal_rotmat)
# object_goal.set_pos(object_goal_pos)
# object_goal.set_rotmat(object_goal_rotmat)
# object_goal.attach_to(base)
fixture = cm.CollisionModel("./objects/tc100.stl")
fixture.set_scale((.001, .001, .001))
fixture.set_rgba([.8, .6, .3, 0.5])
fixture_start = fixture.copy()
fixture_start_pos = np.array([0.800, -.150, 0.780])
fixture_start_rotmat = np.eye(3)
fixture_start_homomat = rm.homomat_from_posrot(fixture_start_pos,
fixture_start_rotmat)
fixture_start_rotmat = fixture_start_homomat[:3, :3]
fixture_start.set_pos(fixture_start_pos)
fixture_start.set_rotmat(fixture_start_rotmat)
fixture_start.attach_to(base)
slopename = "tc71.stl"
slope_high = slope.Slope(z=-0.005,
placement="ping",
size=sinfo.Sloperate[slopename],
show=False)
slopeforcd_high = slope_high.getSlope()
for i, item in enumerate(slopeforcd_high):
item.set_homomat(fixture_start_homomat.dot(item.get_homomat()))
slopeforcd_high[i] = item
bpgm.set_vert_size(.01)
bpgm1.attach_to(base)
bpgm2.attach_to(base)
lsgm = gen_linesegs([[np.array([.1, 0, .01]),
np.array([.01, 0, .01])],
[np.array([.01, 0, .01]),
np.array([.1, 0, .1])],
[np.array([.1, 0, .1]),
np.array([.1, 0, .01])]])
lsgm.attach_to(base)
gen_circarrow(radius=.1, portion=.8).attach_to(base)
gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0, 0,
2])).attach_to(base)
gen_dashframe(pos=np.array([0, 0, 0]), rotmat=np.eye(3)).attach_to(base)
axmat = rm.rotmat_from_axangle([1, 1, 1], math.pi / 4)
gen_frame(rotmat=axmat).attach_to(base)
axmat[:, 0] = .1 * axmat[:, 0]
axmat[:, 1] = .07 * axmat[:, 1]
axmat[:, 2] = .3 * axmat[:, 2]
gen_ellipsoid(pos=np.array([0, 0, 0]), axmat=axmat).attach_to(base)
print(rm.unit_vector(np.array([0, 0, 0])))
pos = np.array([.3, 0, 0])
rotmat = rm.rotmat_from_euler(math.pi / 6, 0, 0)
homomat = rm.homomat_from_posrot(pos, rotmat)
gen_frame_box([.1, .2, .3], homomat).attach_to(base)
base.run()
import robot_sim.robots.cobotta.cobotta as cbt
import manipulation.pick_place_planner as ppp
import motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15])
gm.gen_frame().attach_to(base)
# ground
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7])
ground.set_pos(np.array([0, 0, -.51]))
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()
def get_homomat(self):
npv3 = da.pdv3_to_npv3(self._objpdnp.getPos())
npmat3 = da.pdquat_to_npmat3(self._objpdnp.getQuat())
return rm.homomat_from_posrot(npv3, npmat3)
