def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
use_real=False,
create_sim_world=True,
lft_robot_ip='10.2.0.50',
rgt_robot_ip='10.2.0.51',
pc_ip='10.2.0.100',
cam_pos=np.array([2, 1, 3]),
lookat_pos=np.array([0, 0, 1.1]),
auto_cam_rotate=False):
self.robot_s = ur3ds.UR3Dual(pos=pos, rotmat=rotmat)
self.rrt_planner = rrtc.RRTConnect(self.robot_s)
self.inik_solver = inik.IncrementalNIK(self.robot_s)
self.pp_planner = ppp.PickPlacePlanner(self.robot_s)
if use_real:
self.robot_x = ur3dx.UR3DualX(lft_robot_ip=lft_robot_ip,
rgt_robot_ip=rgt_robot_ip,
pc_ip=pc_ip)
if create_sim_world:
self.sim_world = wd.World(cam_pos=cam_pos,
lookat_pos=lookat_pos,
auto_cam_rotate=auto_cam_rotate)
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])
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)
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,
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.550, -.050, 0.850])
object_goal_rotmat = rm.rotmat_from_axangle(
(0, 0, 1),
np.radians(90)).dot(rm.rotmat_from_axangle((0, 1, 0), np.radians(-90)))
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)
# base.run()
rrtc = rrtc.RRTConnect(robot)
ppp = ppp.PickPlacePlanner(robot)
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_fixture_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)
# robot_instance.gen_meshmodel().attach_to(base)
start_tcp_pos, start_tcp_rotmat = robot_instance.get_init_tcp(component_name)
print(robot_instance.get_jnt_values(component_name))
goal_tcp_pos = start_tcp_pos + np.array([0,-0.000,-0.005])
goal_tcp_rotmat = start_tcp_rotmat
grasp_info_list = gpa.load_pickle_file('tubebig', './', 'graspdata/hnde_tubebig.pickle')
# planner = huplanner.PrimaryPlanner(robot_instance)
# path = planner.gen_linear_moveto( component_name,
# start_tcp_pos,
# start_tcp_rotmat,
# goal_tcp_pos,
# goal_tcp_rotmat,
# obstacle_list=[])
ppp = ppp.PickPlacePlanner(robot_instance)
goal_homomat_list = [object_start_homomat, object_goal_homomat]
for grasp_id in range(len(grasp_info_list)):
grasp_info = grasp_info_list[grasp_id]
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
conf_list, jawwidthlist, objpose_list = ppp.gen_holding_moveto(hand_name = component_name,
objcm = object,
grasp_info = grasp_info,
obj_pose_list = goal_homomat_list,
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,
ad_granularity=.007,
use_rrt=True,
obstacle_list=[],