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
# slopeforcd_high[0].attach_to(base)
# slopeforcd_high[1].attach_to(base)
# slopeforcd_high[2].attach_to(base)
robot = ur3ed.UR3EDual()
component_name = 'rgt_arm'
# robot_meshmodel = robot.gen_meshmodel()
# robot_meshmodel.attach_to(base)
address = this_dir + "/PlacementData"
objname = "test_long_small"
with open(address + "/" + objname + "/" + "placementrotmat.pickle",
"rb") as f:
RotMat = pickle.load(f)
with open(address + "/" + objname + "/" + "stablecklist.pickle",
"rb") as f:
stablecklist = pickle.load(f)
with open(address + "/" + objname + "/" + "placementcom.pickle",
"rb") as f:
comlistall = pickle.load(f)
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:
objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = pickle.load(
file)
keyposes = []
for path in numikrmsmp:
robot_instance.fk("rgt_arm", path[0][1])
start_rgt = robot_instance.get_gl_tcp("rgt_arm")
start_lft = robot_instance.fk("lft_arm", path[0][2])
goal_rgt = robot_instance.fk("rgt_arm", path[-1][1])
goal_lft = robot_instance.fk("lft_arm", path[-1][2])
# keyposes.append(start_rgt*np.pi/180)
if __name__ == '__main__':
import os
base = wd.World(cam_pos=[10, 0, 5], lookat_pos=[0, 0, 1])
base.setFrameRateMeter(True)
gm.gen_frame().attach_to(base)
# 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="convex")
obj_bd_box.set_pos(np.array([.7, 0, 2.7]))
obj_bd_box.start_physics()
base.attach_internal_update_obj(obj_bd_box)
robot_s = ur3ed.UR3EDual()
robot_s.fk(
"both_arm",
np.radians(np.array([-90, -60, -60, 180, 0, 0, 90, -120, 60, 0, 0,
0])))
robot_s.gen_stickmodel().attach_to(base)
robot_s.show_cdprimit()
bd_lnk_list = get_robot_bdmoel(robot_s)
for bdl in bd_lnk_list:
bdl.start_physics()
base.attach_internal_update_obj(bdl)
def update(robot_s, bd_lnk_list, task):
if base.inputmgr.keymap['space'] is True:
la_jnt_values = robot_s.get_jnt_values("lft_arm")
ra_jnt_values = robot_s.get_jnt_values("rgt_arm")