object_holder_gl_goal_pos = np.array([.25, -.05, .08])
# object_holder_gl_goal_pos = np.array([-.15, -.3, .1])
object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos,
object_holder_gl_goal_rotmat)
object_holder_goal_copy = object_holder.copy()
object_holder_goal_copy.set_homomat(obgl_goal_homomat)
object_holder_goal_copy.attach_to(base)
robot_s = cbt.Cobotta()
# robot_s.gen_meshmodel().attach_to(base)
# base.run()
rrtc_s = rrtc.RRTConnect(robot_s)
ppp_s = ppp.PickPlacePlanner(robot_s)
original_grasp_info_list = gpa.load_pickle_file('holder', './',
'cobg_holder_grasps.pickle')
manipulator_name = "arm"
hand_name = "hnd"
start_conf = robot_s.get_jnt_values(manipulator_name)
conf_list, jawwidth_list, objpose_list = \
ppp_s.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=object_holder,
grasp_info_list=original_grasp_info_list,
start_conf=start_conf,
end_conf=start_conf,
goal_homomat_list=[obgl_start_homomat, obgl_goal_homomat],
approach_direction_list=[None, np.array([0, 0, -1])],
approach_distance_list=[.2] * 2,
depart_direction_list=[np.array([0, 0, 1]), None],
depart_distance_list=[.2] * 2)
robot_attached_list = []
object_goal_rotmat = np.eye(3)
object_goal_homomat = rm.homomat_from_posrot(object_goal_pos,
object_goal_rotmat)
object_goal.set_pos(object_goal_pos)
object_goal.set_rotmat(object_goal_rotmat)
object_goal.attach_to(base)
# robot_instance = ur3d.UR3Dual()
robot = ur3ed.UR3EDual()
# robot_instance = sda5.SDA5F()
component_name = 'rgt_arm'
rrtc = rrtc.RRTConnect(robot)
ppp = ppp.PickPlacePlanner(robot)
grasp_info_list = gpa.load_pickle_file('tubebig', './',
'graspdata/hnde_tubebig.pickle')
# manipulator_name = "arm"
hand_name = "rgt_arm"
start_conf = robot.get_jnt_values(component_name)
conf_list, jawwidth_list, objpose_list = \
ppp.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=object,
grasp_info_list=grasp_info_list,
start_conf=start_conf,
end_conf=start_conf,
goal_homomat_list=[object_start_homomat, object_goal_homomat,
object_start_homomat],
approach_direction_list=[None, np.array([0, 0, -1]), None],
approach_distance_list=[.05] * 3,
depart_direction_list=[np.array([0, 0, 1]), None, None],
depart_distance_list=[.05] * 3)
object_box_gl_goal_pos = np.array([.3, -.4, .1])
object_box_gl_goal_rotmat = rm.rotmat_from_euler(math.pi / 3, math.pi / 2,
math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_box_gl_goal_pos,
object_box_gl_goal_rotmat)
object_box_goal_copy = object_box.copy()
object_box_goal_copy.set_homomat(obgl_goal_homomat)
# object_box_goal_copy.attach_to(base)
robot_s = xsm.XArm7YunjiMobile()
# robot_s.gen_meshmodel().attach_to(base)
# base.run()
rrtc_s = rrtc.RRTConnect(robot_s)
ppp_s = ppp.PickPlacePlanner(robot_s)
original_grasp_info_list = gpa.load_pickle_file('box', './',
'xarm_long_box.pickle')
hnd_name = "hnd"
start_conf = robot_s.get_jnt_values(hnd_name)
conf_list, jawwidth_list, objpose_list = \
ppp_s.gen_pick_and_place_motion(hnd_name=hnd_name,
objcm=object_box,
grasp_info_list=original_grasp_info_list,
start_conf=start_conf,
end_conf=start_conf,
goal_homomat_list=[obgl_start_homomat, obgl_goal_homomat],
approach_direction_list=[None, np.array([0, 0, -1])],
approach_distance_list=[.2] * 2,
depart_direction_list=[np.array([0, 0, 1]), None],
depart_distance_list=[.2] * 2)
robot_attached_list = []
object_attached_list = []
tool_initial_homomat = rm.homomat_from_posrot(tool_initial_pos, rotmat)
tool_final.set_rgba([0, 0, 1, 1])
tool_final_pos = np.array([
table2_center[0], table2_center[1],
object_box4.get_pos()[2] + .009 + .11
])
rotmat = np.dot(rm.rotmat_from_axangle([0, 1, 0], math.radians(90)),
rm.rotmat_from_axangle([0, 0, 1], math.radians(90)))
tool_final.set_pos(tool_final_pos)
tool_final.set_rotmat(rotmat)
tool_final_homomat = rm.homomat_from_posrot(tool_final_pos, rotmat)
## rbt_s and grasp list
robot_s = xsm.XArm7YunjiMobile()
rrtc_s = rrtc.RRTConnect(robot_s)
rrtc_planner = rrtdwc.RRTDWConnect(robot_s)
grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
grasp_info_list_tool = gpa.load_pickle_file('box', './', 'xarm_tool.pickle')
jnt_values_initial = robot_s.get_jnt_values(component_name="all")
jnt_values = jnt_values_initial.copy()
jnt_values[0] = tool_final_pos[0] + .8
jnt_values[1] = tool_final_pos[1]
jnt_values[2] = math.pi
robot_s.fk(component_name="all", jnt_values=np.array(jnt_values))
print(jnt_values_initial, jnt_values)
# robot_s.gen_meshmodel().attach_to(base)
## discretize the location and check if the place is available or not
# hnd_name = "hnd"
# ppp_s = ppp.PickPlacePlanner(robot_s)
# goal_homomat_list = [tool_initial_homomat, tool_final_homomat]
# target_obj = object_box4.copy()
# c_pos = target_obj.get_pos()
# object_goal = object_fixture
object_start = object.copy()
object_start_pos = np.array([0.900, -.550, 0.780])
object_start_rotmat = rm.rotmat_from_axangle(
(0, 0, 1), np.radians(180)).dot(
rm.rotmat_from_axangle((1, 0, 0), np.radians(-90)).dot(
rm.rotmat_from_axangle((0, 0, 1), np.radians(180))))
object_start_homomat = rm.homomat_from_posrot(object_start_pos,
object_start_rotmat)
object_start.set_pos(object_start_pos)
object_start.set_rotmat(object_start_rotmat)
# object_start.attach_to(base)
grasp_info_list = gpa.load_pickle_file('test_long', './',
'PlacementData/rtqhe.pickle')
# show_ikfeasible_poses(object_fixture_homomat[:3,:3], object_fixture_pos)
# show_ikfeasible_poses(object_fixture_after_homomat[:3, :3], object_fixture_after_pos)
# base.run()
object_fixture_corner = object.copy()
object_fixture_corner_pos = fixture_start_pos
object_fixture_corner_rotmat = np.eye(3)
object_fixture_corner_homomat = rm.homomat_from_posrot(
object_fixture_corner_pos,
object_fixture_corner_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture_corner.set_homomat(object_fixture_corner_homomat)
object_goal = object.copy()
object_goal_pos = np.array([0.250, -.050, 0.781])
ground.set_pos(np.array([0, 0, -.5]))
ground.attach_to(base)
object_box = cm.gen_box(extent=[.02, .06, .2], rgba=[.7, .5, .3, .7])
object_box_gl_pos = np.array([.5, -.3, .1])
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)
object_box.attach_to(base)
robot_s = xsm.XArmShuidi()
robot_s.gen_meshmodel().attach_to(base)
base.run()
grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_box.pickle')
component_name = "arm"
gripper_s = xag.XArmGripper()
for grasp_info in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
gl_jaw_center_pos = object_box_gl_pos + object_box_gl_rotmat.dot(
jaw_center_pos)
gl_jaw_center_rotmat = object_box_gl_rotmat.dot(jaw_center_rotmat)
gl_hnd_pos = object_box_gl_pos + object_box_gl_rotmat.dot(hnd_pos)
gl_hnd_rotmat = object_box_gl_rotmat.dot(hnd_rotmat)
gripper_s.fix_to(gl_hnd_pos, gl_hnd_rotmat)
if not gripper_s.is_mesh_collided([ground]):
jnt_values = robot_s.ik(component_name,
tgt_pos=gl_jaw_center_pos,
tgt_rotmat=gl_jaw_center_rotmat)
base = wd.World(cam_pos=[0.1, 1, 1],w=960,
h=540, lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
objname = "tubebig"
grippername = "hnde"
object_tube = cm.CollisionModel(f"objects/{objname}.stl")
object_tube.set_rgba([.9, .75, .35, 1])
object_tube.attach_to(base)
# gripper_s = yg.YumiGripper()
gripper_s = hnde.RobotiqHE()
grasp_info_list = gpa.load_pickle_file('tubebig', '.', 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().attach_to(base)
break
# 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)
def update(textNode, task):
if textNode[0] is not None:
textNode[0].detachNode()
object_fixture_rotmat = np.eye(3)
object_fixture_homomat = rm.homomat_from_posrot(object_fixture_pos, object_fixture_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture.set_homomat(object_fixture_homomat)
# object_fixture.attach_to(base)
# object_goal = object_fixture
object_start = object.copy()
object_start_pos = np.array([0.900, -.350, 0.800])
object_start_rotmat = rm.rotmat_from_axangle((1,0,0), np.radians(-90)).dot(rm.rotmat_from_axangle((0,0,1),np.radians(180)))
object_start_homomat = rm.homomat_from_posrot(object_start_pos, object_start_rotmat)
object_start.set_pos(object_start_pos)
object_start.set_rotmat(object_start_rotmat)
# object_start.attach_to(base)
grasp_info_list = gpa.load_pickle_file('test_long', './', 'rtqhe.pickle')
# show_ikfeasible_poses(object_fixture_homomat[:3,:3], object_fixture_pos)
# show_ikfeasible_poses(object_start_homomat[:3, :3], object_start_pos)
# base.run()
object_fixture_corner = object.copy()
object_fixture_corner_pos = fixture_start_pos
object_fixture_corner_rotmat = np.eye(3)
object_fixture_corner_homomat = rm.homomat_from_posrot(object_fixture_corner_pos, object_fixture_corner_rotmat).dot(
da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture_corner.set_homomat(object_fixture_corner_homomat)
object_goal = object.copy()