""" 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)