def pybullet():
import pybullet as pb
pb.connect(pb.DIRECT)
pb.setTimeStep(0.01)
pb.setGravity(0, 0, -9.81)
kitchen = pb.loadURDF(
res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
[0, 0, 0], [0, 0, 0, 1],
0,
True,
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
pr2 = pb.loadURDF(res_pkg_path(
'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml'),
[0, 0, 0], [0, 0, 0, 1],
0,
True,
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
print(kitchen, pr2)
for x in tqdm(range(100)):
cps = pb.getClosestPoints(pr2, kitchen, 40.0, 1, 1)
print('\nClosest points:\n{}'.format(cps))
def test_model_eq(self):
urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
ks1 = ArticulationModel()
ks2 = ArticulationModel()
load_urdf(ks1, Path(urdf_model.name), urdf_model)
load_urdf(ks2, Path(urdf_model.name), urdf_model)
self.assertEquals(ks1, ks2)
def test_double_reload(self):
km = ArticulationModel()
urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
load_urdf(km, Path(urdf_model.name), urdf_model)
km.clean_structure()
eef_frame_1 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose'))
load_urdf(km, Path(urdf_model.name), urdf_model)
km.clean_structure()
eef_frame_2 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose'))
self.assertEquals(eef_frame_1, eef_frame_2)
from kineverse.model.articulation_model import Path
from kineverse.model.geometry_model import GeometryModel
from kineverse.operations.urdf_operations import load_urdf
from kineverse.utils import res_pkg_path
from kineverse.visualization.graph_generator import generate_dependency_graph, plot_graph
from kineverse.visualization.plotting import draw_recorders, split_recorders
from kineverse.urdf_fix import urdf_filler
from sensor_msgs.msg import JointState as JointStateMsg
from urdf_parser_py.urdf import URDF
if __name__ == '__main__':
rospy.init_node('kineverse_kitchen_sandbox')
plot_dir = res_pkg_path('package://kineverse/test/plots')
pub_path = '/opt/ros/{}/lib/robot_state_publisher/robot_state_publisher'.format(
os.environ['ROS_DISTRO'])
with open(
res_pkg_path(
'package://faculty_lounge_kitchen_description/urdfs/kitchen.urdf'
), 'r') as urdf_file:
kitchen_urdf_str = urdf_file.read()
with open(res_pkg_path('package://fetch_description/robots/fetch.urdf'),
'r') as urdf_file:
fetch_urdf_str = urdf_file.read()
kitchen_urdf_model = urdf_filler(URDF.from_xml_string(kitchen_urdf_str))
rospy.set_param('/{}/robot_description'.format(kitchen_urdf_model.name),
out.set_collision_shape(shape)
out.collision_flags = pb.CollisionObject.KinematicObject
out.transform = transform
return out
if __name__ == '__main__':
rospy.init_node('kineverse_bullet_test')
vis = ROSBPBVisualizer('bullet_test', 'world')
kw = pb.KineverseWorld()
kw.force_update_all_aabbs = True
suzanne_obj = pb.load_convex_shape(
res_pkg_path("package://kineverse/meshes/suzanne.obj"))
suzanne_stl = pb.load_convex_shape(
res_pkg_path("package://kineverse/meshes/suzanne.stl"))
cube_shape = pb.BoxShape(pb.Vector3(0.5, 0.5, 0.5))
cube_body = create_object(cube_shape, pb.Transform(1, 0, 0))
cube2_body = create_object(cube_shape, pb.Transform(-1, 0, 0))
#obj_body = create_object(suzanne_obj, pb.Transform(0, 1, 0))
#stl_body = create_object(suzanne_stl, pb.Transform(0,-1, 0))
kw.add_collision_object(cube_body)
kw.add_collision_object(cube2_body)
#kw.add_collision_object(obj_body)
#kw.add_collision_object(stl_body)
cube_body.activate(True)
def main(create_figure=False,
vis_mode=False,
log_csv=True,
min_n_dof=1,
samples=300,
n_observations=25,
noise_lin=0.2,
noise_ang=30,
noise_steps=5):
wait_duration = rospy.Duration(0.1)
vis = ROSBPBVisualizer('ekf_vis', 'world') if vis_mode != 'none' else None
km = GeometryModel()
with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
'r') as urdf_file:
urdf_kitchen_str = urdf_file.read()
kitchen_model = urdf_filler(
URDF.from_xml_string(hacky_urdf_parser_fix(urdf_kitchen_str)))
load_urdf(km, Path('kitchen'), kitchen_model)
km.clean_structure()
km.dispatch_events()
kitchen = km.get_data('kitchen')
tracking_pools = []
for name, link in kitchen.links.items():
symbols = gm.free_symbols(link.pose)
if len(symbols) == 0:
continue
for x in range(len(tracking_pools)):
syms, l = tracking_pools[x]
if len(symbols.intersection(syms)
) != 0: # BAD ALGORITHM, DOES NOT CORRECTLY IDENTIFY SETS
tracking_pools[x] = (syms.union(symbols),
l + [(name, link.pose)])
break
else:
tracking_pools.append((symbols, [(name, link.pose)]))
# tracking_pools = [tracking_pools[7]]
# print('Identified {} tracking pools:\n{}'.format(len(tracking_pools), tracking_pools))
all_ekfs = [
EKFModel(dict(poses), km.get_constraints_by_symbols(symbols))
for symbols, poses in tracking_pools
] # np.eye(len(symbols)) * 0.001
print('Created {} EKF models'.format(len(all_ekfs)))
print('\n'.join(str(e) for e in all_ekfs))
# Sanity constraint
min_n_dof = min(min_n_dof, len(all_ekfs))
iteration_times = []
for u in range(min_n_dof, len(all_ekfs) + 1):
if rospy.is_shutdown():
break
ekfs = all_ekfs[:u]
observed_poses = {}
for ekf in ekfs:
for link_name, _ in ekf.takers:
observed_poses[link_name] = kitchen.links[link_name].pose
names, poses = zip(*sorted(observed_poses.items()))
state_symbols = union([gm.free_symbols(p) for p in poses])
ordered_state_vars = [
s for _, s in sorted((str(s), s) for s in state_symbols)
]
state_constraints = {}
for n, c in km.get_constraints_by_symbols(state_symbols).items():
if gm.is_symbol(c.expr):
s = gm.free_symbols(c.expr).pop()
fs = gm.free_symbols(c.lower).union(gm.free_symbols(c.upper))
if len(fs.difference({s})) == 0:
state_constraints[s] = (float(gm.subs(c.lower, {s: 0})),
float(gm.subs(c.upper, {s: 0})))
state_bounds = np.array([
state_constraints[s]
if s in state_constraints else [-np.pi * 0.5, np.pi * 0.5]
for s in ordered_state_vars
])
state_fn = gm.speed_up(gm.vstack(*poses), ordered_state_vars)
subworld = km.get_active_geometry(state_symbols)
# Generate observation noise
print('Generating R matrices...')
n_cov_obs = 400
full_log = []
dof_iters = []
# EXPERIMENT
for lin_std, ang_std in [(noise_lin, noise_ang * (np.pi / 180.0))]:
# zip(np.linspace(0, noise_lin, noise_steps),
# np.linspace(0, noise_ang * (np.pi / 180.0), noise_steps)):
if rospy.is_shutdown():
break
# INITIALIZE SENSOR MODEL
training_obs = []
state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
observations = state_fn.call2(state)
for _ in range(n_cov_obs):
noisy_obs = {}
for x, noise in enumerate([
t.dot(r) for t, r in zip(
random_normal_translation(len(poses), 0, lin_std),
random_rot_normal(len(poses), 0, ang_std))
]):
noisy_obs[names[x]] = observations[x * 4:x * 4 +
4, :4].dot(noise)
training_obs.append(noisy_obs)
for ekf in ekfs:
ekf.generate_R(training_obs)
# ekf.set_R(np.eye(len(ekf.ordered_vars)) * 0.1)
# Generate figure
gridsize = (4, samples)
plot_size = (4, 4)
fig = plt.figure(figsize=(gridsize[1] * plot_size[0], gridsize[0] *
plot_size[1])) if create_figure else None
gt_states = []
states = [[] for x in range(samples)]
variances = [[] for x in range(samples)]
e_obs = [[] for x in range(samples)]
print('Starting iterations')
for k in tqdm(range(samples)):
if rospy.is_shutdown():
break
state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
gt_states.append(state)
observations = state_fn.call2(state).copy()
gt_obs_d = {
n: observations[x * 4:x * 4 + 4, :4]
for x, n in enumerate(names)
}
subworld.update_world(dict(zip(ordered_state_vars, state)))
if vis_mode == 'iter' or vis_mode == 'io':
vis.begin_draw_cycle('gt', 'noise', 'estimate', 't_n',
't0')
vis.draw_world('gt', subworld, g=0, b=0)
vis.render('gt')
estimates = []
for ekf in ekfs:
particle = ekf.spawn_particle()
estimates.append(particle)
initial_state = dict(
sum([[(s, v) for s, v in zip(ekf.ordered_vars, e.state)]
for e, ekf in zip(estimates, ekfs)], []))
initial_state = np.array(
[initial_state[s] for s in ordered_state_vars])
if initial_state.min() < state_bounds.T[0].min(
) or initial_state.max() > state_bounds.T[1].max():
raise Exception(
'Estimate initialization is out of bounds: {}'.format(
np.vstack([initial_state, state_bounds.T]).T))
initial_delta = state - initial_state
for y in range(n_observations):
# Add noise to observations
noisy_obs = {}
for x, noise in enumerate([
t.dot(r) for t, r in zip(
random_normal_translation(
len(poses), 0, lin_std),
random_rot_normal(len(poses), 0, ang_std))
]):
noisy_obs[names[x]] = observations[x * 4:x * 4 +
4, :4].dot(noise)
if vis_mode in {'iter', 'iter-trail'} or (vis_mode == 'io'
and y == 0):
for n, t in noisy_obs.items():
subworld.named_objects[Path(
('kitchen', 'links', n))].np_transform = t
if vis_mode != 'iter-trail':
vis.begin_draw_cycle('noise')
vis.draw_world('noise', subworld, r=0, g=0, a=0.1)
vis.render('noise')
start_time = time()
for estimate, ekf in zip(estimates, ekfs):
if y > 0:
control = np.zeros(len(ekf.ordered_controls))
estimate.state, estimate.cov = ekf.predict(
estimate.state.flatten(), estimate.cov,
control)
obs_vector = ekf.gen_obs_vector(noisy_obs)
estimate.state, estimate.cov = ekf.update(
estimate.state, estimate.cov,
ekf.gen_obs_vector(noisy_obs))
if vis_mode in {'iter', 'iter-trail'}:
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
else:
obs_vector = ekf.gen_obs_vector(noisy_obs)
for _ in range(1):
h_prime = ekf.h_prime_fn.call2(estimate.state)
obs_delta = obs_vector.reshape(
(len(obs_vector), 1)) - ekf.h_fn.call2(
estimate.state)
estimate.state += (h_prime.T.dot(obs_delta) *
0.1).reshape(
estimate.state.shape)
if vis_mode in {'iter', 'io'}:
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
if vis_mode != 'none' and y == 0:
vis.draw_world('t0', subworld, b=0, a=1)
vis.render('t0')
elif vis_mode in {'iter', 'iter-trail'}:
if vis_mode != 'iter-trail':
vis.begin_draw_cycle('t_n')
vis.draw_world('t_n', subworld, b=0, a=1)
vis.render('t_n')
if log_csv or fig is not None:
e_state_d = dict(
sum([[(s, v)
for s, v in zip(ekf.ordered_vars, e.state)]
for e, ekf in zip(estimates, ekfs)], []))
covs = dict(
sum([[(s, v) for s, v in zip(
ekf.ordered_vars, np.sqrt(np.trace(e.cov)))]
for e, ekf in zip(estimates, ekfs)], []))
e_state = np.hstack([
e_state_d[s] for s in ordered_state_vars
]).reshape((len(e_state_d), ))
if log_csv:
full_log.append(
np.hstack(
([lin_std,
ang_std], state, e_state.flatten(),
np.array([
covs[s] for s in ordered_state_vars
]))))
if fig is not None:
e_obs[k].append(
np.array([
np.abs(
ekf.gen_obs_vector(gt_obs_d) -
ekf.h_fn.call2(e.state)).max()
for e, ekf in zip(estimates, ekfs)
]))
states[k].append(e_state)
variances[k].append(
np.array([covs[s]
for s in ordered_state_vars]))
else:
if vis_mode == 'io':
for estimate, ekf in zip(estimates, ekfs):
subworld.update_world({
s: v
for s, v in zip(ekf.ordered_vars,
estimate.state)
})
vis.draw_world('t_n', subworld, r=0, b=0, a=1)
vis.render('t_n')
dof_iters.append(time() - start_time)
if fig is not None:
axes = [
plt.subplot2grid(gridsize, (y, 0), colspan=1, rowspan=1)
for y in range(gridsize[0])
]
axes = np.array(
sum([[
plt.subplot2grid(gridsize, (y, x),
colspan=1,
rowspan=1,
sharey=axes[y])
for y in range(gridsize[0])
] for x in range(1, gridsize[1])], axes)).reshape(
(gridsize[1], gridsize[0]))
for x, (gt_s, state, variance, obs_delta,
(ax_s, ax_d, ax_o, ax_v)) in enumerate(
zip(gt_states, states, variances, e_obs, axes)):
for y in gt_s:
ax_s.axhline(y, xmin=0.97, xmax=1.02)
ax_s.set_title('State; Sample: {}'.format(x))
ax_d.set_title('Delta from GT; Sample: {}'.format(x))
ax_o.set_title('Max Delta in Obs; Sample: {}'.format(x))
ax_v.set_title('Standard Deviation; Sample: {}'.format(x))
ax_s.plot(state)
ax_d.plot(gt_s - np.vstack(state))
ax_o.plot(obs_delta)
ax_v.plot(variance)
ax_s.grid(True)
ax_d.grid(True)
ax_o.grid(True)
ax_v.grid(True)
fig.tight_layout()
plt.savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker_{}_{}.png'
.format(lin_std, ang_std)))
iteration_times.append(dof_iters)
if log_csv:
df = pd.DataFrame(
columns=['lin_std', 'ang_std'] +
['gt_{}'.format(x) for x in range(len(state_symbols))] +
['ft_{}'.format(x) for x in range(len(state_symbols))] +
['var_{}'.format(x) for x in range(len(state_symbols))],
data=full_log)
df.to_csv(res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker.csv'
),
index=False)
df = pd.DataFrame(
columns=[str(x) for x in range(1,
len(iteration_times) + 1)],
data=np.vstack(iteration_times).T)
df.to_csv(res_pkg_path(
'package://kineverse_experiment_world/test/ekf_object_tracker_performance.csv'
),
index=False)
def test_apply_operation(self):
self.ops_msg = None
self.model_msg = None
def cb_update(ops, model):
self.ops_msg = ops
self.model_msg = model
server = ModelServerForTesting(cb_update)
def cb_srv_get_model(paths, csymbs):
return server.srv_get_model(
bb(paths=paths, constrained_symbols=csymbs))
def cb_srv_get_constraints(csymbs):
return server.srv_get_constraints(bb(symbols=csymbs))
op_client = OperationsClientForTesting(None, True)
m_client = ModelClientForTesting(cb_srv_get_model,
cb_srv_get_constraints, None)
m_client.has_data('testbot')
urdf_model = URDF.from_xml_file(
res_pkg_path('package://kineverse/urdf/testbot.urdf'))
load_urdf(op_client, urdf_model.name, urdf_model)
op_list = op_client.apply_changes()
req = bb(operations=op_list)
server.srv_apply_operations(req)
server.srv_apply_operations(req)
op_client.cb_operations_update(self.ops_msg)
with open('update.json', 'w') as f:
json.dump(dict(
zip(self.model_msg.update.paths, self.model_msg.update.data)),
f,
indent=2)
m_client.cb_model_update(self.model_msg)
o_model = server.km.get_data('testbot')
t_model = m_client.get_data('testbot')
with open('original.json', 'w') as f:
json.dump(o_model, f, indent=True)
with open('transmitted.json', 'w') as f:
json.dump(t_model, f, indent=True)
self.assertEquals(o_model.name, t_model.name)
l = o_model.links['arm_mount_link']
lt = t_model.links['arm_mount_link']
for n, l in o_model.links.items():
if n not in t_model.links:
print('Link {} missing from transmitted model'.format(n))
continue
lt = t_model.links[n]
if l == lt:
print('Equality check passed for link {}'.format(n))
print('Types: {} {}'.format(type(l), type(lt)))
print('parent: {}'.format(lt.parent == l.parent))
print('pose: {}'.format(lt.pose == l.pose))
print('to_parent: {}'.format(lt.to_parent == l.to_parent))
print('geometry: {}'.format(lt.geometry == l.geometry))
print('collision: {}'.format(lt.collision == l.collision))
print('inertial: {}\n---'.format(lt.inertial == l.inertial))
else:
print('Equality check failed for link {}'.format('arm_mount_link'))
print('Types: {} {}'.format(type(l), type(lt)))
print('parent: {}'.format(lt.parent == l.parent))
print('pose: {}\n{}'.format(lt.pose == l.pose, l.pose - lt.pose))
print('to_parent: {}\n{}'.format(lt.to_parent == l.to_parent,
l.to_parent - lt.to_parent))
print('geometry: {}'.format(lt.geometry == l.geometry))
print('collision: {}'.format(lt.collision == l.collision))
print('inertial: {}'.format(lt.inertial == l.inertial))
self.assertEquals(o_model.joints, t_model.joints)
self.assertEquals(o_model, t_model)
if __name__ == '__main__':
rospy.init_node('kineverse_roomba_uploader')
if len(sys.argv) < 3:
print(
'Usage: [urdf file] wheel_radius wheel_distance [fill_collision_tags]'
)
exit()
elif len(sys.argv) < 4:
urdf_model = URDF.from_xml_string(
rospy.get_param('/robot_description'))
radius = float(sys.argv[1])
distance = float(sys.argv[2])
else:
urdf_model = URDF.from_xml_file(res_pkg_path(sys.argv[1]))
radius = float(sys.argv[2])
distance = float(sys.argv[3])
# Fill collision geometry if demanded
if len(sys.argv) >= 5 and (sys.argv[4].lower() == 'true'
or sys.argv[4] == '1'):
urdf_model = urdf_filler(urdf_model)
op_client = OperationsClient(EventModel, True)
load_urdf(op_client, urdf_model.name, urdf_model)
op_client.apply_operation_before(
'create world', 'create {}'.format(urdf_model.name),
CreateComplexObject(Path('world'), Frame('')))
from kineverse.utils import res_pkg_path
from kineverse.visualization.graph_generator import generate_modifications_graph,\
generate_dependency_graph, \
plot_graph
from kineverse.visualization.plotting import draw_recorders, split_recorders
from sensor_msgs.msg import JointState as JointStateMsg
from trajectory_msgs.msg import JointTrajectory as JointTrajectoryMsg
from trajectory_msgs.msg import JointTrajectoryPoint as JointTrajectoryPointMsg
from urdf_parser_py.urdf import URDF
if __name__ == '__main__':
rospy.init_node('kineverse_sandbox')
plot_dir = res_pkg_path('package://kineverse/test/plots')
pub_path = '/opt/ros/{}/lib/robot_state_publisher/robot_state_publisher'.format(
os.environ['ROS_DISTRO'])
with open(res_pkg_path('package://fetch_description/robots/fetch.urdf'),
'r') as urdf_file:
urdf_str = urdf_file.read()
rospy.set_param('/robot_description', urdf_str)
js_pub = rospy.Publisher('/joint_states', JointStateMsg, queue_size=1)
traj_pup = rospy.Publisher('/fetch/commands/joint_trajectory',
JointTrajectoryMsg,
queue_size=1)
tf_broadcaster = tf.TransformBroadcaster()
'l_forearm_roll_joint': 0.0,
'l_wrist_flex_joint': -1.05,
'r_elbow_flex_joint': -2.1213,
'r_shoulder_lift_joint': 1.2963,
'r_shoulder_pan_joint': 0.0,
'r_upper_arm_roll_joint': 0.0,
'r_forearm_roll_joint': 0.0,
'r_wrist_flex_joint': -1.05,
'torso_lift_joint': 0.16825
}
closer = ObsessiveObjectCloser(
Path('iai_oven_area'),
Path('pr2/links/r_gripper_r_finger_tip_link'),
Path('pr2/links/head_mount_link'),
'/tracked/state',
'/pr2',
resting_pose=resting_pose)
while not rospy.is_shutdown():
rospy.sleep(1000)
if closer.pushing_controller is not None and PANDA_LOGGING:
recW, recB, recC, recs = convert_qp_builder_log(
closer.pushing_controller)
draw_recorders([recW, recB, recC] + [
r for _, r in sorted(recs.items())
], 1, 12, 6).savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/keep_contact.png'))
) in symbolic_types and u_const_name not in generated_constraints:
new_goals[u_const_name] = SoftConstraint(
less_than(c.upper, 0), 1000, goal.weight, c.upper)
generated_constraints.add(u_const_name)
elif diff_sign[s] < 0 and type(
c.lower
) in symbolic_types and l_const_name not in generated_constraints:
new_goals[l_const_name] = SoftConstraint(
-1000, -greater_than(c.lower, 0), goal.weight,
c.lower)
generated_constraints.add(l_const_name)
final_goals.update(
lock_explorer(km, state, new_goals, generated_constraints))
return final_goals
if __name__ == '__main__':
scenario = LockboxOpeningGenerator()
scenario.run(0.19)
draw_recorders([
scenario.value_recorder, scenario.vel_recorder,
scenario.symbol_recorder
], 4.0 / 9.0, 10, 3).savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/plots/lockbox_opening.png'
))
Path(f'{prefix}/links/handle')))
km.apply_operation(
f'add lock {door_position}',
ConditionalDoorHandleConstraints(door_position, handle_position,
math.pi * 0.01, math.pi * 0.15))
if __name__ == '__main__':
rospy.init_node('kineverse_door_opening')
visualizer = ROSBPBVisualizer('~visualization', base_frame='world')
km = GeometryModel()
with open(
res_pkg_path(
'package://kineverse_experiment_world/urdf/iiwa_wsg_50.urdf'),
'r') as f:
iiwa_urdf = urdf_filler(
URDF.from_xml_string(hacky_urdf_parser_fix(f.read())))
load_urdf(km, Path('iiwa'), iiwa_urdf)
km.clean_structure()
km.dispatch_events()
door_x, door_y = [gm.Position(f'localization_{x}') for x in 'xy']
create_door(km,
Path('door'),
0.5,
0.35,
to_world_tf=gm.translation3(door_x, door_y, 0.1))
if args.link is not None:
robot_link = args.link
elif robot == 'pr2':
robot_link = 'r_gripper_r_finger_tip_link'
else:
robot_link = 'gripper_link' # 'r_gripper_finger_link'
camera_link = args.camera
if camera_link is None:
if args.robot == 'fetch':
camera_link = 'head_camera_rgb_optical_frame'
elif args.robot == 'pr2':
camera_link = 'head_mount_kinect_rgb_optical_frame'
plot_dir = res_pkg_path('package://kineverse/test/plots')
# Loading of models
if robot != 'pr2':
with open(
res_pkg_path(
'package://{r}_description/robots/{r}.urdf'.format(
r=robot)), 'r') as urdf_file:
urdf_str = hacky_urdf_parser_fix(urdf_file.read())
else:
with open(
res_pkg_path(
'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml'
), 'r') as urdf_file:
urdf_str = hacky_urdf_parser_fix(urdf_file.read())
from kineverse.motion.min_qp_builder import TypedQPBuilder as TQPB, \
SoftConstraint as SC, \
ControlledValue as CV, \
generate_controlled_values
from kineverse.motion.integrator import CommandIntegrator
from kineverse.visualization.bpb_visualizer import ROSBPBVisualizer
from kineverse.visualization.plotting import draw_recorders, split_recorders
from kineverse.visualization.trajectory_visualizer import TrajectoryVisualizer
from urdf_parser_py.urdf import URDF
if __name__ == '__main__':
rospy.init_node('microwave_sandbox')
micro_urdf = URDF.from_xml_file(
res_pkg_path('package://kineverse/urdf/microwave.urdf'))
km = EventModel()
#km = GeometryModel()
load_urdf(km, Path('microwave'), micro_urdf)
traj_vis = TrajectoryVisualizer(ROSBPBVisualizer('/bullet_test', '/map'))
traj_vis.add_articulated_object(micro_urdf, km.get_data('microwave'))
door_position = km.get_data('microwave/joints/door_joint').position
button_position = km.get_data('microwave/joints/button_joint').position
button_min = -0.02
door_velocity = get_diff_symbol(door_position)
door_accel = get_diff_symbol(door_velocity)
point_of_release = 0.8 * button_min
button_pushed = less_than(button_position, point_of_release)
#node.init(mode='direct')
req = AddRigidObjectRequest()
req.geom_type = 'box'
req.extents.x = req.extents.y = 10
req.extents.z = 1
req.pose.position.z = -0.5
req.pose.orientation.w = 1
req.name = 'floor'
node.srv_add_rigid_body(req) # This is dumb!
tmp_urdf = open('/tmp/temp_urdf.urdf', 'w') #tempfile.NamedTemporaryFile()
filled_model = urdf_filler(
URDF.from_xml_file(
res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf')))
tmp_urdf.write(filled_model.to_xml_string())
tmp_urdf.close()
#tmp_urdf.write(urdf_filler(URDF.from_xml_file(res_pkg_path('package://faculty_lounge_kitchen_description/urdfs/kitchen.urdf'))).to_xml_string())
#snd_urdf = URDF.from_xml_file('/tmp/temp_urdf.urdf')
req = AddURDFRequest()
req.urdf_path = '/tmp/temp_urdf.urdf' #'package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'
req.name = 'iai_kitchen'
req.fixed_base = True
req.pose.orientation.w = 1
node.srv_add_urdf(req) # This is reeeeeeally stupid!
angle = math.pi * 0
help='Number of steps from lowest to highest noise.')
parser.add_argument('--out',
'-o',
type=str,
default='tracker_results_n_dof.csv',
help='Name of the resulting csv file.')
args = parser.parse_args()
rospy.init_node('kineverse_tracking_node')
tracker = TrackerNode('/tracked/state',
'/pose_obs',
args.step,
args.max_iter,
use_timer=False)
with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
'r') as urdf_file:
urdf_kitchen_str = urdf_file.read()
kitchen_model = urdf_filler(URDF.from_xml_string(urdf_kitchen_str))
load_urdf(tracker.km_client, Path('iai_oven_area'), kitchen_model)
tracker.km_client.clean_structure()
tracker.km_client.dispatch_events()
groups = [
[('iai_oven_area/links/room_link', 'iai_oven_area/room_link'),
('iai_oven_area/links/fridge_area', 'iai_oven_area/fridge_area'),
('iai_oven_area/links/fridge_area_footprint',
'iai_oven_area/fridge_area_footprint'),
('iai_oven_area/links/fridge_area_lower_drawer_handle',
def load_convex_mesh_shape(pkg_filename):
return pb.load_convex_shape(res_pkg_path(pkg_filename))
def bpb():
import numpy as np
import kineverse.bpb_wrapper as pb
import kineverse.model.geometry_model as gm
import kineverse.operations.urdf_operations as urdf
from kineverse.urdf_fix import urdf_filler
from urdf_parser_py.urdf import URDF
km = gm.GeometryModel()
with open(
res_pkg_path(
'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml'
), 'r') as urdf_file:
urdf.load_urdf(km, 'pr2',
urdf_filler(URDF.from_xml_string(urdf_file.read())))
with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
'r') as urdf_file:
urdf.load_urdf(km, 'kitchen',
urdf_filler(URDF.from_xml_string(urdf_file.read())))
km.clean_structure()
km.dispatch_events()
kitchen = km.get_data('kitchen')
pr2 = km.get_data('pr2')
joint_symbols = {
j.position
for j in kitchen.joints.values() if hasattr(j, 'position')
}
joint_symbols |= {
j.position
for j in pr2.joints.values() if hasattr(j, 'position')
}
coll_world = km.get_active_geometry(joint_symbols)
robot_parts = {
n: o
for n, o in coll_world.named_objects.items() if n[:4] == 'pr2/'
}
batch = {o: 2.0 for o in robot_parts.values()}
print(
'Benchmarking by querying distances for {} objects. Total object count: {}.\n'
.format(len(batch), len(coll_world.names)))
dur_update = []
dur_distances = []
# print('Mesh files:\n{}'.format('\n'.join(sorted({pb.pb.get_shape_filename(s) for s in sum([o.collision_shape.child_shapes for o in coll_world.collision_objects], [])}))))
# print('Objects:\n{}'.format('\n'.join(['{}:\n {}\n {} {}'.format(n,
# o.transform.rotation,
# o.transform.origin,
# pb.pb.get_shape_filename(o.collision_shape.get_child(0)))
# for n, o in coll_world.named_objects.items()])))
for x in tqdm(range(100)):
start = Time.now()
coll_world.update_world({
s: v
for s, v in zip(joint_symbols, np.random.rand(len(joint_symbols)))
})
dur_update.append(Time.now() - start)
start = Time.now()
distances = coll_world.closest_distances(batch)
dur_distances.append(Time.now() - start)
dur_update_mean = sum([d.to_sec() for d in dur_update]) / len(dur_update)
dur_distances_mean = sum([d.to_sec()
for d in dur_distances]) / len(dur_distances)
print('Update mean: {}\nUpdate max: {}\n'
'Distances mean: {}\nDistances max: {}\n'.format(
dur_update_mean,
max(dur_update).to_sec(),
dur_distances_mean,
max(dur_distances).to_sec(),
))
from kineverse.utils import res_pkg_path
if __name__ == '__main__':
eef = point3(1,0,0)
goal = rotation3_rpy(0, -0.8, 2.56) * point3(1, 0, 0)
rpy_model = rotation3_rpy(Position('roll'), Position('pitch'), Position('yaw')) * eef
axis = vector3(Position('x'), Position('y'), 0) #Position('z'))
ax_model = rotation3_axis_angle(axis / (norm(axis) + 1e-4), norm(axis)) * eef
goal_rpy = SC(-norm(goal - rpy_model), -norm(goal - rpy_model), 1, norm(goal - rpy_model))
goal_ax = SC(-norm(goal - ax_model), -norm(goal - ax_model), 1, norm(goal - ax_model))
rpy_integrator = CommandIntegrator(TQPB({}, {'rpy': goal_rpy},
{str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in rpy_model.free_symbols}),
recorded_terms={'dist rpy': norm(goal - rpy_model)})
ax_integrator = CommandIntegrator(TQPB({}, {'ax': goal_ax},
{str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in ax_model.free_symbols}),
recorded_terms={'dist ax': norm(goal - ax_model)})
rpy_integrator.restart('Convergence Test: RPY vs AxAngle')
rpy_integrator.run()
ax_integrator.restart('Convergence Test: RPY vs AxAngle')
ax_integrator.run()
draw_recorders([rpy_integrator.recorder, ax_integrator.recorder,
rpy_integrator.sym_recorder, ax_integrator.sym_recorder], 1, 4, 4).savefig('{}/rpy_vs_axis_angle.png'.format(res_pkg_path('package://kineverse_experiment_world/test')))
def test_load(self):
urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
ks = ArticulationModel()
load_urdf(ks, Path(urdf_model.name), urdf_model)
def srv_image_to_object(req):
res = ImageToObjectResponseMsg()
res.objects = fake_observation()
return res
if __name__ == '__main__':
if len(sys.argv) < 2:
print('SDF file needed.')
exit(0)
kin_links = None
for a in sys.argv[1:]:
if not ':=' in a:
world_path = res_pkg_path(a)
print('Loading world file {}'.format(world_path))
sdf = SDF(load_xml(world_path))
kin_links = world_to_links(sdf.worlds.values()[0], Path('bla'),
False)
break
if kin_links is None:
print('No SDF file was given.')
exit(0)
rospy.init_node('fake_observation')
robot_name = rospy.get_param('~robot_name', 'fetch')
width = rospy.get_param('~width', 640)
height = rospy.get_param('~height', 480)
near = rospy.get_param('~near', 0.2)
'wrist_roll_joint': 0.0,
'shoulder_pan_joint': 1.32,
'elbow_flex_joint': 1.72,
'forearm_roll_joint': 0.0,
'upperarm_roll_joint': -0.2,
'wrist_flex_joint': 1.66,
'shoulder_lift_joint': 1.4,
'torso_lift_joint': 0.2
}
use_omni = False
if __name__ == '__main__':
rospy.init_node('kineverse_sandbox')
plot_dir = res_pkg_path('package://kineverse/test/plots')
with open(res_pkg_path('package://fetch_description/robots/fetch.urdf'),
'r') as urdf_file:
urdf_str = urdf_file.read()
with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
'r') as urdf_file:
urdf_kitchen_str = urdf_file.read()
urdf_model = URDF.from_xml_string(urdf_str)
kitchen_model = urdf_filler(URDF.from_xml_string(urdf_kitchen_str))
traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format(
urdf_model.name),
JointTrajectoryMsg,