traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
urdf_model.name),
JointTrajectoryMsg,
queue_size=1)
kitchen_traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
kitchen_model.name),
JointTrajectoryMsg,
queue_size=1)
# KINEMATIC MODEL
km = GeometryModel()
load_urdf(km, Path('fetch'), urdf_model)
load_urdf(km, Path('kitchen'), kitchen_model)
km.clean_structure()
km.apply_operation_before('create world', 'create fetch',
CreateComplexObject(Path('world'), Frame('')))
if use_omni:
base_op = create_omnibase_joint_with_symbols(
Path('world/pose'), Path('fetch/links/base_link/pose'),
Path('fetch/joints/to_world'), vector3(0, 0, 1), 1.0, 0.6,
Path('fetch'))
else:
base_op = create_roomba_joint_with_symbols(
Path('world/pose'), Path('fetch/links/base_link/pose'),
Path('fetch/joints/to_world'), vector3(0, 0, 1), vector3(1, 0, 0),
1.0, 0.6, Path('fetch'))
km.apply_operation_after('connect world base_link',
'create fetch/base_link', base_op)
km.clean_structure()
km.dispatch_events()
JointTrajectoryMsg,
queue_size=1)
kitchen_traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
kitchen_model.name),
JointTrajectoryMsg,
queue_size=1)
# KINEMATIC MODEL
km = GeometryModel()
load_urdf(km, Path(robot), urdf_model)
load_urdf(km, Path('kitchen'), kitchen_model)
# create_nobilia_shelf(km, Path('nobilia'), gm.frame3_rpy(0, 0, 0.4,
# gm.point3(1.2, 0, 0.8)))
km.clean_structure()
km.apply_operation_before('create world', 'create {}'.format(robot),
ExecFunction(Path('world'), Frame, ''))
base_joint_path = Path(f'{robot}/joints/to_world')
# Insert base to world kinematic
if use_omni:
insert_omni_base(km, Path(robot), urdf_model.get_root())
else:
insert_diff_base(km,
Path(robot),
urdf_model.get_root(),
wheel_radius=0.12 * 0.5,
wheel_distance=0.3748,
wheel_vel_limit=17.4)
km.dispatch_events()
traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
urdf_model.name),
JointTrajectoryMsg,
queue_size=1)
kitchen_traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
kitchen_model.name),
JointTrajectoryMsg,
queue_size=1)
# KINEMATIC MODEL
km = GeometryModel()
load_urdf(km, Path(robot), urdf_model)
load_urdf(km, Path('kitchen'), kitchen_model)
km.clean_structure()
km.apply_operation_before('create world', 'create {}'.format(robot),
CreateComplexObject(Path('world'), Frame('')))
if robot == 'pr2' or use_omni:
base_op = create_omnibase_joint_with_symbols(
Path('world/pose'),
Path('{}/links/{}/pose'.format(robot, urdf_model.get_root())),
Path('{}/joints/to_world'.format(robot)), vector3(0, 0, 1), 1.0,
0.6, Path(robot))
else:
base_op = create_roomba_joint_with_symbols(
Path('world/pose'),
Path('{}/links/{}/pose'.format(robot, urdf_model.get_root())),
Path('{}/joints/to_world'.format(robot)), vector3(0, 0, 1),
vector3(1, 0, 0), 1.0, 0.6, Path(robot))
km.apply_operation_after(
'connect world {}'.format(urdf_model.get_root()),