import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm 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])
import modeling.collision_model as cm import visualization.panda.world as wd import modeling.geometric_model as gm import utils if __name__ == '__main__': base = wd.World(cam_pos=[1.7, -1, .7], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) this_dir, this_filename = os.path.split(__file__) file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl") frame_bottom = cm.CollisionModel(file_frame) frame_bottom.set_rgba([.55, .55, .55, 1]) frame_bottom.attach_to(base) table_plate = cm.gen_box(extent=[.405, .26, .003]) table_plate.set_pos([0.07 + 0.2025, .055, .0015]) table_plate.set_rgba([.87, .87, .87, 1]) table_plate.attach_to(base) file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl") tip_rack = utils.Rack96(file_tip_rack) tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1]) tip_rack.set_pose(pos=np.array([.25, 0.0, .003]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) tip_rack.attach_to(base) file_tip = os.path.join(this_dir, "objects", "tip.stl") tip = cm.CollisionModel(file_tip) tip.set_rgba([200 / 255, 180 / 255, 140 / 255, 1]) for id_x in range(8):
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xsm import robot_sim.end_effectors.gripper.xarm_gripper.xarm_gripper as xag import manipulation.approach_depart_planner as adp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7]) object_box_gl_pos = np.array([.5, -.3, .35]) object_box_gl_rotmat = np.eye(3) object_box.set_pos(object_box_gl_pos) object_box.set_rotmat(object_box_gl_rotmat) gm.gen_frame().attach_to(object_box) robot_s = xsm.XArmShuidi() rrtc_s = rrtc.RRTConnect(robot_s) adp_s = adp.ADPlanner(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
from direct.gui.OnscreenText import OnscreenText from panda3d.core import TextNode import numpy as np import basis.robot_math as rm import modeling.geometric_model as gm import robot_sim.robots.ur3_dual.ur3_dual as ur3d import robot_sim.robots.ur3e_dual.ur3e_dual as ur3ed import robot_sim.robots.sda5f.sda5f as sda5 import motion.probabilistic.rrt_connect as rrtc import pickle if __name__ == '__main__': base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1]) gm.gen_frame().attach_to(base) phoxicam = cm.gen_box(extent=np.array([1.000, .43, .1]), homomat=np.eye(4)) phoxicam.set_pos( np.array([100 + 200, 0, 1360 + 175 + 200 + 90 / 2]) / 1000) phoxicam.attach_to(base) object = cm.CollisionModel("./objects/lshape-30.stl") object.set_scale([0.001, 0.001, 0.001]) object.set_pos(np.array([1.05, -.3, 1.3])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) component_name = 'rgt_arm' # robot_instance = ur3d.UR3Dual() robot_instance = ur3ed.UR3EDual() # robot_instance = sda5.SDA5F() # robot_instance.gen_meshmodel().attach_to(base) with open("experiment180-1123.pickle", "rb") as file:
import math import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm 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' robot_s = nxt.Nextage() # start_pos = np.array([.4, 0, .2]) # start_rotmat = rm.rotmat_from_euler(0, math.pi * 2 / 3, -math.pi / 4) start_pos = np.array([.4, .1, .1]) start_rotmat = rm.rotmat_from_axangle([0,1,0], -math.pi/2) start_conf = robot_s.ik(component_name, start_pos, start_rotmat) # goal_pos = np.array([.3, .5, .7]) # goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi) goal_pos = np.array([.3, .5, .6]) goal_rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi/2).dot(rm.rotmat_from_axangle([0, 1, 0], math.pi)) goal_conf = robot_s.ik(component_name, goal_pos, goal_rotmat)
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)
# but when I change them to meters, the number lowers down to 1fps or less. I am wondering if I did a wrong option. import modeling.collision_model as cm import modeling.dynamics.bullet.bdbody as bbd import visualization.panda.world as wd import math import basis.robot_math as rm import numpy as np base = wd.World(cam_pos=np.array([2, 0, 2]), lookat_pos=np.array([0, 0, 0]), toggle_debug=False) # PlaneD homomat = np.eye(4) homomat[:3, 3] = np.array([0, 0, -.05]) planecm = cm.gen_box(extent=[1, 1, .1], homomat=homomat) # planenode = bch.genBulletCDMesh(planecm) planenode = bbd.BDBody(planecm, cdtype='convex', dynamic=False) 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.attach_to(base) planecm.set_homomat(planenode.get_homomat()) cm.gm.gen_frame().attach_to(base) # for i in range(5): # print("....")
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import robot_sim.end_effectors.gripper.xarm_gripper.xarm_gripper as xag base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) object_box = cm.gen_box(extent=[.02, .06, .7]) object_box.set_rgba([.7, .5, .3, .7]) object_box.attach_to(base) # hnd_s gripper_s = xag.XArmGripper() grasp_info_list = gpa.plan_grasps(gripper_s, object_box, openning_direction='loc_y', max_samples=7, min_dist_between_sampled_contact_points=.03) gpa.write_pickle_file('box', grasp_info_list, './', 'xarm_long_box.pickle') for grasp_info in grasp_info_list: jaw_width, gl_jaw_center_pos, gl_jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gripper_s.fix_to(hnd_pos, hnd_rotmat) gripper_s.jaw_to(jaw_width) gripper_s.gen_meshmodel().attach_to(base) base.run()
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import numpy as np import basis.robot_math as rm import robot_sim.robots.nextage.nextage as nxt import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xsm import motion.probabilistic.rrt_connect as rrtc import math import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_differential_wheel_connect as rrtdwc base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) ## show human human = nxt.Nextage() rotmat = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2) human.fix_to(np.array([0, -1.5, 1]), rotmat=rotmat) gm.gen_arrow(spos=np.array([0, -1.5, 1]), epos=np.array([0, -1.5, 0]), thickness=0.01, rgba=np.array([.5, 0, 0, 1])).attach_to(base) human.gen_stickmodel().attach_to(base) ## show table2 table2_center = np.array([-1.3, .6, .483]) table2_bottom = cm.gen_box(extent=[1.08, .42, .06], rgba=[150 / 255, 154 / 255, 152 / 255, 1])
# box_0.attach_to(base) # suction_s = suction.MVFLN40() # loc_pos_box = np.array([.1, 0, .02]) # loc_rotmat_box = rm.rotmat_from_euler(math.pi, 0, 0) # gl_pos_box = box_0.get_pos() + box_0.get_rotmat().dot(loc_pos_box) # gl_rotmat_box = box_0.get_rotmat().dot(loc_rotmat_box) # suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box) # suction_s.gen_meshmodel(rgba=[.55, .55, .55, .3]).attach_to(base) # gm.gen_stick( # suction_s.pos, # np.array([0,0,1]),rgba=[1,0,0,.3]).attach_to(base) rtq_s = rtq.Robotiq85() box = cm.gen_box(np.array([.3, .3, .04])) box.set_rgba([153 / 255, 183 / 255, 1, 1]) box.set_pos(np.array([0, 0, .02])) # box.set_rotmat(rm.rotmat_from_axangle([0, 1, 0], -math.pi / 12)) grasp_info_list = gpa.plan_grasps( rtq_s, box, angle_between_contact_normals=math.radians(175), openning_direction='loc_y') # for grasp_info in grasp_info_list: # jaw_width, gl_jaw_center_pos, gl_jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info # rtq_s.fix_to(hnd_pos, hnd_rotmat) # rtq_s.jaw_to(jaw_width) # rtq_s.gen_meshmodel().attach_to(base) grasp_info = grasp_info_list[11] jaw_width, gl_jaw_center_pos, gl_jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
self.earth.set_pos(pos=pos) def attach_to(self, base): self.cup.attach_to(base) self.earth.attach_to(base) def copy(self): return copy.deepcopy(self) base = wd.World(cam_pos=[4.2, 4.2, 2.5], lookat_pos=[0, 0, .7], auto_cam_rotate=True) frame = gm.GeometricModel(initor="meshes/frame.stl") frame.set_rgba(rgba=aluminium_rgba) frame.attach_to(base) bottom_box = cm.gen_box(extent=[.88, 1.68, .45], rgba=board_rgba) bottom_box.set_pos(pos=np.array([0, 0, .22])) bottom_box.attach_to(base) top_box = cm.gen_box(extent=[.88, 1.68, .3], rgba=board_rgba) top_box.set_pos(pos=np.array([0, 0, 1.65])) top_box.attach_to(base) cup = Cup() cup_pos_x = [0.09 - .44, 0.23 - .44, 0.37 - .44, 0.51 - .44, 0.65 - .44, 0.79 - .44] cup_pos_y = [.09 - .84, .24 - .84, .39 - .84, .54 - .84, .69 - .84, .84 - .84, .99 - .84, 1.14 - .84, 1.29 - .84, 1.44 - .84, 1.59 - .84] # cup_pos_x = [ 0.23 - .44, 0.51 - .44, 0.79 - .44] # cup_pos_y = [.24 - .84, .54 - .84, .84 - .84, 1.14 - .84, # 1.44 - .84] cup_pos_z = .37
import math import numpy as np from basis import robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xss import motion.probabilistic.rrt_differential_wheel_connect as rrtdwc base = wd.World(cam_pos=[10, 1, 5], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object box_homo = np.eye(4) box_homo[:3, 3] = np.array([0, 0, .5]) object_box = cm.gen_box(np.array([.3, .3, 1]), homomat=box_homo, rgba=[.57, .57, .5, 1]) object_box.set_pos(np.array([1.9, -1, 0])) object_box.attach_to(base) # object2 object_box2 = object_box.copy() object_box2.set_pos(np.array([1.9, -.5, 0])) object_box2.attach_to(base) # object3 object_box3 = object_box.copy() object_box3.set_pos(np.array([.9, -.5, 0])) object_box3.attach_to(base) # object4 object_box4 = object_box.copy() object_box4.set_pos(np.array([.9, -1, 0])) object_box4.attach_to(base)
# 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(
world.setGravity(0, 0, -9.81) world.setQuickStepNumIterations(100) world.setErp(.2) world.setCfm(1e-3) # The surface table is needed for autoCollide world.initSurfaceTable(1) world.setSurfaceEntry(0, 0, 150, 0.0, 9.1, 0.9, 0.00001, 0.0, 0.002) # Create a space and add a contactgroup to it to add the contact joints space = OdeSimpleSpace() space.setAutoCollideWorld(world) contactgroup = OdeJointGroup() space.setAutoCollideJointGroup(contactgroup) box = cm.gen_box(extent=[.3, .3, .3]) # Add a random amount of boxes boxes = [] for i in range(randint(5, 10)): # Setup the geometry new_box = box.copy() new_box.set_pos( np.array([random() * 10 - 5, random() * 10 - 5, 1 + random()])) new_box.set_rgba([random(), random(), random(), 1]) new_box.set_rotmat( rm.rotmat_from_euler(random() * math.pi / 4, random() * math.pi / 4, random() * math.pi / 4)) new_box.attach_to(base)