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 pickle
import robot_sim.end_effectors.gripper.robotiq85.robotiq85 as rtq85
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], auto_cam_rotate=True)
gm.gen_frame().attach_to(base)
object_bunny = cm.CollisionModel("objects/pblcm_cropped_8_2_20000_cvt.stl")
object_bunny.set_rgba([.9, .75, .35, .3])
object_bunny.attach_to(base)
# hnd_s
gripper_s = rtq85.Robotiq85()
# base.run()
# gripper_s.gen_meshmodel(toggle_jntscs=True, toggle_tcpcs=True).attach_to(base)
# base.run()
grasp_info_list = gpa.plan_grasps(gripper_s,
object_bunny,
openning_direction='loc_y',
max_samples=100,
min_dist_between_sampled_contact_points=.01)
for grasp_info in grasp_info_list:
aw_width, gl_jaw_center, gl_jaw_rotmat, hnd_pos, hnd_rotmat = grasp_info
gripper_s.fix_to(hnd_pos, hnd_rotmat)
gripper_s.jaw_to(aw_width)
gripper_s.gen_meshmodel().attach_to(base)
# break
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 robot_sim.end_effectors.gripper.robotiq85_gelsight.robotiq85_gelsight as rtq85
import math
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
obj = cm.CollisionModel("tubebig.stl")
obj.set_rgba([.9, .75, .35, .3])
obj.attach_to(base)
# hnd_s
gripper_s = rtq85.Robotiq85Gelsight()
grasp_info_list = gpa.plan_grasps(
gripper_s,
obj,
angle_between_contact_normals=math.radians(170),
openning_direction='loc_y',
max_samples=100,
min_dist_between_sampled_contact_points=.01,
rotation_interval=math.radians(180))
print(len(grasp_info_list))
for grasp_info in grasp_info_list:
# print(grasp_info)
jaw_width, gl_jaw_center, gl_jaw_rotmat, hnd_pos, hnd_rotmat = grasp_info
gripper_s.jaw_to(jaw_width)
gripper_s.fix_to(hnd_pos, hnd_rotmat)
gripper_s.gen_meshmodel().attach_to(base)
base.run()
import robot_sim.end_effectors.grippers.cobotta_gripper.cobotta_gripper as cg
import modeling.collision_model as cm
import modeling.geometric_model as gm
import numpy as np
import math
base = wd.World(cam_pos=np.array([.5, .5, .5]), lookat_pos=np.array([0, 0, 0]))
gm.gen_frame().attach_to(base)
objcm = cm.CollisionModel("objects/holder.stl")
objcm.attach_to(base)
# base.run()
hnd_s = cg.CobottaGripper()
# hnd_s.gen_meshmodel().attach_to(base)
# base.run()
grasp_info_list = gp.plan_grasps(hnd_s,
objcm,
angle_between_contact_normals=math.radians(175),
openning_direction='loc_y',
rotation_interval=math.radians(15),
max_samples=20,
min_dist_between_sampled_contact_points=.001,
contact_offset=.001)
gp.write_pickle_file(objcm_name="holder",
grasp_info_list=grasp_info_list,
file_name="cobg_holder_grasps.pickle")
for grasp_info in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
hnd_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width)
hnd_s.gen_meshmodel().attach_to(base)
base.run()
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import robot_sim.end_effectors.grippers.yumi_gripper.yumi_gripper as yg
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtqhe
base = wd.World(cam_pos=[1, 1, 1],w=960,
h=540, lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
object_tube = cm.CollisionModel("objects/test_long.STL")
object_tube.set_rgba([.9, .75, .35, 1])
object_tube.set_scale([0.001, 0.001, 0.001])
object_tube.attach_to(base)
# hnd_s
gripper_s = rtqhe.RobotiqHE()
grasp_info_list = gpa.plan_grasps(gripper_s, object_tube,
angle_between_contact_normals=math.radians(177),
openning_direction='loc_x',
rotation_interval=math.radians(22.5/2),
max_samples=30, min_dist_between_sampled_contact_points=.01,
contact_offset=.001)
gpa.write_pickle_file('test_long', grasp_info_list, './', 'rtqhe.pickle')
for grasp_info in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width)
gripper_s.gen_meshmodel(rgba=(0,1,0,0.1)).attach_to(base)
# jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat
base.run()
# 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
rtq_s.fix_to(hnd_pos, hnd_rotmat)
rtq_s.jaw_to(jaw_width)
rtq_s.gen_meshmodel().attach_to(base)
box.attach_to(base)
base.run()
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as hnde
base = wd.World(cam_pos=[1, 1, 1], w=960, h=540, lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
objname = "tubebig"
grippername = "hnde"
# object
object_tube = cm.CollisionModel(f"objects/{objname}.stl")
object_tube.set_rgba([.9, .75, .35, 1])
object_tube.attach_to(base)
# hnd_s
# gripper_s = yg.YumiGripper()
gripper_s = hnde.RobotiqHE()
grasp_info_list = gpa.plan_grasps(
gripper_s,
object_tube,
angle_between_contact_normals=math.radians(177),
openning_direction='loc_x',
max_samples=15,
min_dist_between_sampled_contact_points=.005,
contact_offset=.005)
gpa.write_pickle_file('tubebig', grasp_info_list, '.',
f'graspdata/{grippername}_{objname}.pickle')
for grasp_info in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width)
gripper_s.gen_meshmodel(rgba=(1, 0, 0, 0.01)).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 robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
# object_box = cm.gen_box(extent=[.02, .06, 1])
# object_box.set_rgba([.7, .5, .3, .7])
# object_box.attach_to(base)
object_bunny = cm.CollisionModel("objects/bunnysim.stl")
object_bunny.set_rgba([.9, .75, .35, .3])
object_bunny.attach_to(base)
# hnd_s
gripper_s = rtq85.Robotiq85()
grasp_info_list = gpa.plan_grasps(gripper_s, object_bunny, max_samples=100, min_dist_between_sampled_contact_points=.01)
for grasp_info in grasp_info_list:
aw_width, gl_jaw_center, hnd_pos, hnd_rotmat = grasp_info
gripper_s.fix_to(hnd_pos, hnd_rotmat)
gripper_s.jaw_to(aw_width)
gripper_s.gen_meshmodel().attach_to(base)
base.run()