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)
