"""
self.__changableobslist.append(objcm)
objcm.reparentTo(nodepath)
objcm.setMat(base.pg.npToMat4(rot, pos))
def removechangableobs(self, objcm):
if objcm in self.__changableobslist:
objcm.remove()
if __name__ == '__main__':
import utils.robotmath as rm
import numpy as np
from panda3d.core import *
base = pandactrl.World(camp=[2700,300,2700], lookatp=[0,0,1000])
env = Env()
env.reparentTo(base.render)
objcm = env.loadobj("bunnysim.stl")
objcm.setColor(.2,.5,0,1)
objcm.setPos(400,-200,1200)
objcm.reparentTo(base.render)
objcm.showcn()
obscmlist = env.getstationaryobslist()
for obscm in obscmlist:
obscm.showcn()
objpos = np.array([400,-300,1200])
objrot = rm.rodrigues([0,1,0], 45)
objcm2 = env.loadobj("housing.stl")
objmat4list.append(objmat4worlda)
objmat4list.append(objmat4)
objmat4list.append(objmat4)
objmat4list.append(objmat4handx)
objmat4list.append(objmat4handxworldz)
return [objmat4list, numikrlist, jawwidth]
if __name__=='__main__':
gdb = db.GraspDB()
nxtrobot = nxt.NxtRobot()
handpkg = rtq85nm
#base = pandactrl.World(camp=[300,-2000,1000], lookatp=[300,0,0])
base = pandactrl.World(camp=[5000, -1000, 1000], lookatp=[0, 0, 0])
# ttube.stl
this_dir = "E:/project/manipulation/regrasp_onworkcell/dropsimulation"
# objpath = os.path.join(this_dir, "objects", "ttube.stl")
# workcellpath = os.path.join(this_dir, "objects", "workcell22.stl")
objpath = os.path.join(this_dir, "objects", "t2tube.stl")
#objpath = os.path.join(this_dir, "objects", "planerearstay.stl")
#objpath = os.path.join(this_dir, "objects", "CameraFrontCase.stl")
workcellpath = os.path.join(this_dir, "objects", "ipadbox.stl")
#objpath = os.path.join(this_dir, "objects", "boxobject.stl")
#workcellpath = os.path.join(this_dir, "objects", "camerafrontcase_workcell.stl")
regrip = regriptpp.RegripTpp(objpath,nxtrobot, handpkg, gdb)
author: weiwei
date: 20170609
"""
robot.movearmfk(point)
iscollided = cdchecker.isCollided(robot, obstaclelist)
robot.goinitpose()
if iscollided:
return True
else:
return False
if __name__ == '__main__':
base = pandactrl.World()
robot = hrp5n.Hrp5NRobot()
handpkg = hrp5threenm
robotmesh = hrp5nmesh.Hrp5NMesh(handpkg)
robotball = hrp5nball.Hrp5NBall()
# cdchecker = cdck.CollisionChecker(robotmesh)
cdchecker = cdck.CollisionCheckerBall(robotball)
start = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
goal = [0.0, 45.0, -20.0, 0.0, -150.0, 0.0, 0.0, 0.0, 0.0]
start = [0.0, 45.0, -20.0, 0.0, -150.0, 0.0, 0.0, 0.0, 0.0]
goal = [0.0, 90.0, -60.0, -70.0, -100.0, -30.0, 0.0, 0.0, 0.0]
start = [0.0, 45.0, -20.0, 0.0, -150.0, 0.0, 0.0, 0.0, 0.0]
goal = [0.0, -20.0, -60.0, -70.0, -100.0, -30.0, 0.0, 0.0, 0.0]
world.doPhysics(globalClock.getDt())
# result = base.world.contactTestPair(bcollidernp.node(), lftcollidernp.node())
# result1 = base.world.contactTestPair(bcollidernp.node(), ilkcollidernp.node())
# result2 = base.world.contactTestPair(lftcollidernp.node(), ilkcollidernp.node())
# print result
# print result.getContacts()
# print result1
# print result1.getContacts()
# print result2
# print result2.getContacts()
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
return task.cont
base = pandactrl.World(lookatpos=[0, 0, 0])
base.world = BulletWorld()
base.taskMgr.add(updateworld,
"updateworld",
extraArgs=[base.world],
appendTask=True)
sck918hnd = Sck918(jawwidth=10, ftsensoroffset=0)
# sck918hnd.setJawwidth(25)
sck918hnd.gripAt(0, 0, 0, 0, 0, 1, 30)
sck918hnd.reparentTo(base.render)
base.pggen.plotAxis(base.render)
rmathnd = sck918hnd.getMat()
# base.pggen.plotAxis(base.render, spos = rmathnd.getRow3(3), pandamat3=rmathnd.getUpper3())
# base.run()
# base = pandactrl.World()
steplength)
newpolygonxoncurvesSeg12 = mgc.cvtpolygonDict2List(
polygonxoncurvedictSeg12, scale, steplength)
# fgr12
onedcurvedict12 = mgc.cvtcurveList2Dict(onedcurves12, scale, steplength)
onedcurvedict12, polygonxoncurvedict12 = \
mgc.removeCollisiononOnedCurves(onedcurvedict12, cobtsdictSeg0, polygonxoncurvedict12)
onedcurvedict12, polygonxoncurvedict12 = \
mgc.removeCollisiononOnedCurves(onedcurvedict12, cobtsdictSeg1, polygonxoncurvedict12)
newonedcurves12 = mgc.cvtcurveDict2List(onedcurvedict12, scale, steplength)
newpolygonxoncurves12 = mgc.cvtpolygonDict2List(polygonxoncurvedict12,
scale, steplength)
# plot
polygoncenter = [polygon.centroid.x, polygon.centroid.y, 360 * scale / 2.0]
base = pc.World(camp=[-5000, 2000, 10000], lookatp=polygoncenter)
for height in range(0, int(360 * scale), steplength):
rotedcobsegpolygonsnp = stb.genPolygonsnp(cobtsdictSeg0[height],
height,
color=[0.3, .3, .3, .6],
thickness=1)
for polygonnp in rotedcobsegpolygonsnp:
polygonnp.reparentTo(base.render)
# for height in range(0,int(360*scale),steplength):
# rotedcobsegpolygonsnp = stb.genPolygonsnp(cobtsdictSeg1[height], height, color = [0.3,.3,.3,.6], thickness=1)
# for polygonnp in rotedcobsegpolygonsnp:
# polygonnp.reparentTo(base.render)
# for height in range(0,int(360*scale),steplength):
# rotedcobsegpolygonsnp = stb.genPolygonsnp(cobtsdict1[height], height, color = [0.3,.3,.3,.6], thickness=1)
if __name__ == "__main__":
# show in panda3d
from direct.showbase.ShowBase import ShowBase
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
# from direct.filter.CommonFilters import CommonFilters
from robotsim.ur3dual import ur3dual
from robotsim.ur3dual import ur3dualmesh
from manipulation.grip.robotiq85 import rtq85
base = pandactrl.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700])
robot = ur3dual.Ur3DualRobot()
robotmesh = ur3dualmesh.Ur3DualMesh()
rgthnd = rtq85.Rtq85()
lfthnd = rtq85.Rtq85()
robotmnp = robotmesh.genmnp(robot)
robotmnp.reparentTo(base.render)
base.pggen.plotAxis(base.render)
cdchecker = CollisionChecker(robotmesh)
print(cdchecker.isSelfCollided(robot))
bullcldrnp = base.render.attachNewNode("bulletcollider")
debugNode = BulletDebugNode('Debug')
theta = math.degrees(anglewvalue * dtime)
# rbd.rotmat = np.dot(pg.mat3ToNp(Mat4.rotateMat(theta, Vec3(axis[0], axis[1], axis[2])).getUpper3()), rbd.rotmat)
# rbd.rotmat = np.dot(tf.rotation_matrix(theta*math.pi/180.0, axis)[:3, :3], rbd.rotmat)
rbd.rotmat = np.dot(rm.rodrigues(axis, theta), rbd.rotmat)
rbd.anglew = rbd.anglew + rbd.danglew * dtime
return angularmomentum
if __name__ == "__main__":
import os
import math
from panda3d.core import *
import pandaplotutils.pandageom as pg
import pandaplotutils.pandactrl as pc
base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0])
rbd = Rigidbody(pos=np.array([0, 0, 0]),
rotmat=np.identity(3),
inertiatensor=np.array([[80833.3, 0.0, 0.0],
[0.0, 68333.3, 0.0],
[0.0, 0.0, 14166.7]]))
rbd.anglew = np.array([1, 1, 1])
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "models", "box.egg"))
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
rbdnp = []
import numpy as np
import pandaplotutils.pandactrl as pc
import pandaplotutils.pandageom as pg
import utils.robotmath as rm
base = pc.World(camp=[1000, -300, 700], lookatp=[0, 0, 0], w=1000, h=1000)
pggen = pg.PandaGeomGen()
pggen.plotAxis(base.render)
pggen.plotBox(base.render,
pos=[0, 0, 0],
x=100,
y=100,
z=50,
rgba=[1, 1, .3, 1])
#
rotmat = rm.rodrigues([1, 1, 1], 30)
pandarotmat = pg.cvtMat4(rotmat, np.array([0, 0, 0]))
pggen.plotAxis(base.render, pandamat4=pandarotmat)
base.run()
poselist = []
for string in f1:
s = " ".join(string.split()).replace(" ", ",")
# s = " ".join(strlist.split())
# print(s)
list1 = eval(s)
poselist.append(
[list1[1], list1[2], list1[3], list1[4], list1[5], list1[6]])
return poselist
if __name__ == '__main__':
poselist = readfile("data20200227_164419.csv", start=1)
base = pc.World(camp=[0, 3000, 0],
lookatp=[200, 0, 0],
up=[0, 0, 1],
fov=40,
w=1920,
h=1080)
for pose in poselist:
# rot_joint = rm.euler_matrix(-135.523, -60.799, -52.4953)
# pos_joint = np.array([399.265, 86.996, -176.666])
# print(pose[3], pose[4], pose[5])
print(pose)
rot_joint = rm.euler_matrix(pose[3], pose[4], pose[5])
# print("rot_joint =", rot_joint)
pos_joint = np.array([pose[0], pose[1], pose[2]])
joint_pose_to_base = rm.homobuild(pos_joint, rot_joint)
# print("joint_pose_to_base =", joint_pose_to_base)
rot_obj0 = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
# print(rot_obj)
import environment.collisionmodel as cm
# import environment.bulletcdhelper as bch
import modeling.dynamics.bullet.bdbody as bbd
import pandaplotutils.pandactrl as pc
import copy
import trimesh
import utiltools.robotmath as rm
import numpy as np
base = pc.World(camp=[5000, 0, 5000], lookatpos=[0, 0, 0], toggledebug=True)
# PlaneD
plane = trimesh.primitives.Box(box_extents=[1000, 1000, 100],
box_center=[0, 0, -50])
planecm = cm.CollisionModel(plane)
# planenode = bch.genBulletCDMesh(planecm)
planenode = bbd.BDTriangleBody(planecm, dynamic=True)
planemat = np.eye(4)
planemat[:3, 3] = planemat[:3, 3] + np.array([0, 0, 0])
planenode.set_homomat(planemat)
planenode.setMass(0)
planenode.setRestitution(0)
planenode.setFriction(1)
base.physicsworld.attachRigidBody(planenode)
planecm.reparentTo(base.render)
planecm.setMat(base.pg.np4ToMat4(planenode.get_homomat()))
base.pggen.plotAxis(base.render)
# for i in range(5):
# print("....")
# currentmat = planenode.gethomomat()
'tabletopplacementrotmathandxworldz']
objmat4list.append(objmat4worldaworldz)
objmat4list.append(objmat4worlda)
objmat4list.append(objmat4)
objmat4list.append(objmat4)
objmat4list.append(objmat4handx)
objmat4list.append(objmat4handxworldz)
return [objmat4list, numikrlist, jawwidth]
if __name__ == '__main__':
gdb = db.GraspDB()
nxtrobot = nxt.NxtRobot()
handpkg = rtq85nm
base = pandactrl.World(camp=[300, -2000, 1000], lookatp=[300, 0, 0])
# ttube.stl
this_dir, this_filename = os.path.split(__file__)
# objpath = os.path.join(os.path.split(this_dir)[0], "grip", "objects", "ttube.stl")
# objpath = os.path.join(os.path.split(this_dir)[0], "grip", "objects", "tool.stl")
# objpath = os.path.join(os.path.split(this_dir)[0], "grip", "objects", "planewheel.stl")
# objpath = os.path.join(os.path.split(this_dir)[0], "grip", "objects", "planelowerbody.stl")
objpath = os.path.join(
os.path.split(os.path.split(this_dir)[0])[0], "grip", "objects",
"planefrontstay.stl")
# objpath = os.path.join(os.path.split(this_dir)[0], "grip", "objects", "planerearstay.stl")
regrip = regriptpp.RegripTpp(objpath, nxtrobot, handpkg, gdb)
# ttube
startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
if chan.getNumEntries() > 0:
return True
else:
return False
if __name__ == '__main__':
import utiltools.robotmath as rm
import numpy as np
import environment.bunrisettingfree as bunrisettingfree
import pandaplotutils.pandactrl as pc
import robotsim.ur3dual.ur3dual as ur3dualsim
import robotsim.ur3dual.ur3dualmesh as ur3dualsimmesh
import manipulation.grip.robotiq85.robotiq85 as rtq85
base = pc.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000])
env = bunrisettingfree.Env()
env.reparentTo(base.render)
objcm = env.loadobj("bunnysim.stl")
objcm.setColor(.2, .5, 0, 1)
objcm.setPos(400, -200, 1200)
objcm.reparentTo(base.render)
objcm.showcn()
obscmlist = env.getstationaryobslist()
for obscm in obscmlist:
obscm.showcn()
objpos = np.array([400, -300, 1200])
objrot = rm.rodrigues([0, 1, 0], 45)
objcm2 = env.loadobj("housing.stl")
from robotsim.nextage import nxtmesh
from motionplanning.rrt import rrtconnect as rrtc
import motionplanning.ctcallback as ctcb
import motionplanning.collisioncheckerball as cdck
from robotsim.nextage import nxtball
import motionplanning.smoother as sm
from readfiles import readyaml
import time
import math
from direct.task import Task
import bldgfsettingnear
import robotcon.rpc.nxtrobot.nxtrobot_client as nxturx
if __name__ == "__main__":
base = pandactrl.World(camp=[3500, -300, 1500], lookatp=[0, 0, 1000])
env = bldgfsettingnear.Env(base)
env.reparentTo(base.render)
nxtrobot = nxt.NxtRobot()
robotball = nxtball.NxtBall()
lfthand = sck918.Sck918(ftsensoroffset=0)
rgthand = sck918.Sck918(ftsensoroffset=0)
robotmesh = nxtmesh.NxtMesh(
lfthand=lfthand,
rgthand=rgthand,
)
nxtmesh = robotmesh.genmnp(nxtrobot,
jawwidthrgt=50.0,
jawwidthlft=50.0,
togglejntscoord=False)
