def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
name="xarm7_shuidi_mobile",
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# agv
self.agv = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(5),
name='agv') # 1-3 x,y,theta; 4-5 dummy
self.agv.jnts[1]['loc_pos'] = np.zeros(3)
self.agv.jnts[1]['type'] = 'prismatic'
self.agv.jnts[1]['loc_motionax'] = np.array([1, 0, 0])
self.agv.jnts[1]['motion_rng'] = [0.0, 5.0]
self.agv.jnts[2]['loc_pos'] = np.zeros(3)
self.agv.jnts[2]['type'] = 'prismatic'
self.agv.jnts[2]['loc_motionax'] = np.array([0, 1, 0])
self.agv.jnts[2]['motion_rng'] = [-3.0, 3.0]
self.agv.jnts[3]['loc_pos'] = np.zeros(3)
self.agv.jnts[3]['loc_motionax'] = np.array([0, 0, 1])
self.agv.jnts[3]['motion_rng'] = [-math.pi, math.pi]
self.agv.jnts[4]['loc_pos'] = np.array([0, .0, .277]) # dummy
self.agv.jnts[5]['loc_pos'] = np.zeros(3) # dummy
self.agv.jnts[6]['loc_pos'] = np.array([0, .0, .168862])
self.agv.lnks[3]['name'] = 'agv'
self.agv.lnks[3]['loc_pos'] = np.array([0, 0, 0])
self.agv.lnks[3]['mesh_file'] = os.path.join(this_dir, 'meshes',
'shuidi_agv.stl')
self.agv.lnks[3]['rgba'] = [.7, .7, .7, 1.0]
self.agv.lnks[4]['mesh_file'] = os.path.join(this_dir, 'meshes',
'battery.stl')
self.agv.lnks[4]['rgba'] = [.35, .35, .35, 1.0]
self.agv.lnks[5]['mesh_file'] = os.path.join(this_dir, 'meshes',
'battery_fixture.stl')
self.agv.lnks[5]['rgba'] = [.55, .55, .55, 1.0]
self.agv.tgtjnts = [1, 2, 3]
self.agv.reinitialize()
# arm
arm_homeconf = np.zeros(7)
arm_homeconf[1] = -math.pi / 3
arm_homeconf[3] = math.pi / 12
arm_homeconf[5] = -math.pi / 12
self.arm = xa.XArm7(pos=self.agv.jnts[-1]['gl_posq'],
rotmat=self.agv.jnts[-1]['gl_rotmatq'],
homeconf=arm_homeconf,
name='arm',
enable_cc=False)
# ft sensor
self.ft_sensor = jl.JLChain(pos=self.arm.jnts[-1]['gl_posq'],
rotmat=self.arm.jnts[-1]['gl_rotmatq'],
homeconf=np.zeros(0),
name='ft_sensor_jl')
self.ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .065])
self.ft_sensor.lnks[0]['name'] = "xs_ftsensor"
self.ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.ft_sensor.lnks[0]['collision_model'] = cm.gen_stick(
spos=self.ft_sensor.jnts[0]['loc_pos'],
epos=self.ft_sensor.jnts[1]['loc_pos'],
thickness=.075,
rgba=[.2, .3, .3, 1],
sections=24)
self.ft_sensor.reinitialize()
# gripper
self.hnd = xag.XArmGripper(
pos=self.ft_sensor.jnts[-1]['gl_posq'],
rotmat=self.ft_sensor.jnts[-1]['gl_rotmatq'],
name='hnd_s',
enable_cc=False)
# tool center point
self.arm.jlc.tcp_jnt_id = -1
self.arm.jlc.tcp_loc_rotmat = self.ft_sensor.jnts[-1][
'loc_rotmat'].dot(self.hnd.jaw_center_rotmat)
self.arm.jlc.tcp_loc_pos = self.ft_sensor.jnts[-1][
'loc_pos'] + self.arm.jlc.tcp_loc_rotmat.dot(
self.hnd.jaw_center_pos)
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
# component map
self.manipulator_dict['arm'] = self.arm
self.manipulator_dict['ftsensor'] = self.arm
self.manipulator_dict[
'hnd'] = self.arm # specify which hand is a gripper installed to
self.hnd_dict['arm'] = self.hnd
self.hnd_dict['ftsensor'] = self.hnd
self.hnd_dict['hnd'] = self.hnd
self.ft_sensor_dict['arm'] = self.ft_sensor
self.ft_sensor_dict['ftsensor'] = self.ft_sensor
self.ft_sensor_dict['hnd'] = self.ft_sensor
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')
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)
conf_path, jw_path = adp_s.gen_approach_motion(
component_name,
gl_jaw_center_pos,
gl_jaw_center_rotmat,
start_conf=robot_s.get_jnt_values(component_name),
approach_direction=gl_jaw_center_rotmat[:, 2],
approach_distance=.2)
if conf_path is None:
continue
else:
print(k, len(v))
grasp_info_list = data[objcm_name]
return grasp_info_list
except:
raise ValueError("File or data not found!")
if __name__ == '__main__':
import os
import basis
import robot_sim.end_effectors.gripper.xarm_gripper.xarm_gripper as xag
import modeling.collision_model as cm
import visualization.panda.world as wd
base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0])
gripper_s = xag.XArmGripper(enable_cc=True)
objpath = os.path.join(basis.__path__[0], 'objects', 'block.stl')
objcm = cm.CollisionModel(objpath)
objcm.attach_to(base)
objcm.show_localframe()
grasp_info_list = define_grasp_with_rotation(
gripper_s,
objcm,
gl_jaw_center_pos=np.array([0, 0, 0]),
gl_jaw_center_z=np.array([1, 0, 0]),
gl_jaw_center_y=np.array([0, 1, 0]),
jaw_width=.04,
gl_rotation_ax=np.array([0, 0, 1]))
for grasp_info in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
gic = gripper_s.copy()