def cb_obj_model_changed(self, model):
print('URDF model changed')
if type(model) is not URDFRobot:
raise Exception(
'Path "{}" does not refer to a urdf_robot. Type is "{}"'.
format(self.urdf_path, type(model)))
self.obj = model
self.possible_targets = [(Path(joint.child), joint.position)
for jname, joint in self.obj.joints.items()
if joint.type in goal_joints]
self.target_symbol_map = dict(self.possible_targets)
self.obj_world = self.km.get_active_geometry(
{s
for _, s in self.possible_targets if cm.is_symbol(s)})
new_state = {
s: 0.0
for _, s in self.possible_targets if cm.is_symbol(s)
}
self.obj_js_aliases = {
str(Path(erase_type(s))[len(self.urdf_path):]): s
for s in new_state.keys()
}
print('Object aliases:\n{}'.format('\n'.join([
'{:>20}: {}'.format(k, v) for k, v in self.obj_js_aliases.items()
])))
self.inverse_js_aliases.update(
{erase_type(v): k
for k, v in self.obj_js_aliases.items()})
self.state.update(new_state)
def init(self, path, frame):
self.frame = frame
attrs = collect_paths(self.frame, Path('frame'))
super(CreateRelativeFrame, self).init('Frame',
[str(a) for a in attrs],
parent=Path(self.frame.parent),
**{str(a): path + a[1:] for a in attrs})
def insert_diff_base(km,
robot_path,
root_link,
r_wheel_position,
l_wheel_position,
world_frame='world',
wheel_radius=0.06,
wheel_distance=0.4,
wheel_vel_limit=0.6):
if not km.has_data(world_frame):
km.apply_operation_before(f'create {world_frame}', f'create {robot_path}', ExecFunction(Path(world_frame), Frame, ''))
base_joint_path = robot_path + Path(f'joints/to_{world_frame}')
base_op = ExecFunction(base_joint_path,
create_diff_drive_joint_with_symbols,
CPath(f'{world_frame}/pose'),
CPath(robot_path + CPath(f'links/{root_link}/pose')),
wheel_radius,
wheel_distance,
wheel_vel_limit,
r_wheel_position,
l_wheel_position,
CPath(robot_path))
km.apply_operation_after(f'create {base_joint_path}',
f'create {robot_path}/links/{root_link}', base_op)
km.apply_operation_after(f'connect {world_frame} {robot_path}/links/{root_link}',
f'create {base_joint_path}',
CreateAdvancedFrameConnection(base_joint_path,
Path(world_frame),
robot_path + Path(f'links/{root_link}')))
km.clean_structure()
def from_dict(cls, init_dict):
km = GeometryModel()
urdf = urdf_filler(URDF.from_xml_string(rospy.get_param('/robot_description')))
load_urdf(km, Path(urdf.name), urdf)
km.clean_structure()
km.dispatch_events()
base_frame = init_dict['reference_frame']
eefs = [Endeffector.from_dict(km.get_data(Path(urdf.name)), base_frame, d) for d in init_dict['links']]
return cls(km, Path(urdf.name), eefs)
def register_on_model_changed(self, path, callback):
if type(path) == str:
path = Path(path)
self.km.register_on_model_changed(path, callback)
if path not in self.tracked_paths:
self._start_tracking(path)
def load_model(km, model_path, reference_frame, x, y, z, yaw):
origin_pose = gm.frame3_rpy(0, 0, yaw, gm.point3(x, y, z))
if model_path != 'nobilia':
urdf_model = load_urdf_file(model_path)
load_urdf(km,
Path(urdf_model.name),
urdf_model,
reference_frame,
root_transform=origin_pose)
return urdf_model.name
else:
create_nobilia_shelf(km, Path('nobilia'), origin_pose, parent_path=Path(reference_frame))
return 'nobilia'
def get_data(self, path):
if type(path) == str:
path = Path(path)
if path not in self.tracked_paths:
self._start_tracking(path)
return self.km.get_data(path)
def init(self, transform_path, from_frame, to_frame):
self.tf_obj = Transform(str(from_frame), str(to_frame), None)
attrs = collect_paths(self.tf_obj, Path('transform'))
super(CreateRelativeTransform, self).init('Relative Transform',
[str(a) for a in attrs],
frame_a=from_frame,
frame_b=to_frame,
**{str(a): transform_path + a[1:] for a in attrs})
def insert_omni_base(km, robot_path, root_link, world_frame='world', lin_vel=1.0, ang_vel=0.6):
if not km.has_data(world_frame):
km.apply_operation_before(f'create {world_frame}', f'create {robot_path}', ExecFunction(Path(world_frame), Frame, ''))
base_joint_path = robot_path + Path(f'joints/to_{world_frame}')
base_op = ExecFunction(base_joint_path,
create_omnibase_joint_with_symbols,
CPath(f'{world_frame}/pose'),
CPath(robot_path + Path(f'links/{root_link}/pose')),
gm.vector3(0,0,1),
1.0, 0.6, CPath(robot_path))
km.apply_operation_after(f'create {base_joint_path}',
f'create {robot_path}/links/{root_link}', base_op)
km.apply_operation_after(f'connect {world_frame} {robot_path}/links/{root_link}',
f'create {base_joint_path}',
CreateAdvancedFrameConnection(base_joint_path,
Path(world_frame),
robot_path + Path(f'links/{root_link}')))
km.clean_structure()
def get_root_frames(frame_dict):
if len(frame_dict) > 0:
if type(frame_dict.keys()[0]) == str:
return {
k: f
for k, f in frame_dict.items() if f.parent not in frame_dict
}
return {
k: f
for k, f in frame_dict.items() if Path(f.parent) not in frame_dict
}
return {}
def track(self, model_path, alias):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
start_tick = len(self.tracked_poses) == 0 and self._use_timer
if model_path not in self.tracked_poses:
syms = [
Position(Path(model_path) + (x, ))
for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']
]
def rf(msg, state):
state[syms[0]] = msg.position.x
state[syms[1]] = msg.position.y
state[syms[2]] = msg.position.z
state[syms[3]] = msg.orientation.x
state[syms[4]] = msg.orientation.y
state[syms[5]] = msg.orientation.z
state[syms[6]] = msg.orientation.w
def process_model_update(data):
self._generate_pose_constraints(model_path, data)
axis = vector3(*syms[:3])
self.aliases[alias] = model_path
self.tracked_poses[model_path] = TrackerEntry(
frame3_quaternion(*syms), rf, process_model_update)
self._unintialized_poses.add(model_path)
if type(self.km_client) == ModelClient:
self.km_client.register_on_model_changed(
Path(model_path), process_model_update)
else:
process_model_update(self.km_client.get_data(model_path))
if start_tick:
self.timer = rospy.Timer(rospy.Duration(1.0 / 50),
self.cb_tick)
def register_on_model_changed(self, path, callback):
if type(path) is str:
path = Path(path)
if self.__in_dispatch_mode:
self.__callback_additions.append((path, callback))
else:
if path not in self.model_change_callbacks:
self.model_change_callbacks[path] = set()
self.model_change_callbacks[path].add(callback)
if callback not in self._callback_path_registry:
self._callback_path_registry[callback] = set()
self._callback_path_registry[callback].add(path)
def save_to_file(self, f, subpath=Path('')):
self.clean_structure()
if len(subpath) != 0:
json.dump([{
'op': top.op,
'tag': top.tag
} for top in self.get_history_of(subpath)], f)
else:
json.dump([{
'op': chunk.operation,
'tag': t
} for (_, t), chunk in zip(
sorted([(s, t) for t, s in self.timeline_tags.items()]),
self.operation_history.chunk_history)], f)
def _dispatch_model_events(pdict, data, key, call_tracker=set()):
for cb in pdict.value:
if cb not in call_tracker:
cb(data)
call_tracker.add(cb)
if len(key) == 0:
for k, p in pdict.items():
_dispatch_model_events(p,
Path(k).get_data_no_throw(data), key,
call_tracker)
else:
_dispatch_model_events(pdict.get_sub_dict(key[:1]),
key[:1].get_data_no_throw(data), key[1:],
call_tracker)
def _update_model(self, msg):
if len(msg.paths) != len(msg.data):
raise Exception('Message path and data arrays need to be of equal length.\n Paths: {}\n Data: {}'.format(len(msg.paths), len(msg.data)))
for str_path, data in zip(msg.paths, msg.data):
path = Path(str_path)
if path in self.tracked_paths:
print('Updating {}'.format(path))
self.km.set_data(path, json.loads(data))
for cmsg in msg.constraints:
c = decode_constraint_filtered(cmsg, self.tracked_symbols)
if c is not None:
self.km.add_constraint(cmsg.name, c)
elif self.km.has_constraint(cmsg.name):
self.km.remove_constraint(cmsg.name)
def dispatch_events(self):
static_objects = []
static_poses = []
dynamic_poses = {}
for str_k in self._collision_objects:
k = Path(str_k)
if k in self._callback_batch:
#print('{} is a collision link and has changed in the last update batch'.format(k))
if self.has_data(k):
#print('{} was changed'.format(k))
link = self.get_data(k)
pose_expr = link.pose # * link.collision.to_parent
symbol_set = set()
if type(pose_expr) is GM:
symbol_set = symbol_set.union(pose_expr.diff_symbols)
pose_expr = pose_expr.to_sym_matrix()
symbol_set |= pose_expr.free_symbols.union(
{get_diff_symbol(s)
for s in pose_expr.free_symbols})
self._co_pose_expr[str_k] = pose_expr
self._co_symbol_map[str_k] = symbol_set
if len(symbol_set) > 0:
for s in symbol_set:
if s not in self._symbol_co_map:
self._symbol_co_map[s] = set()
self._symbol_co_map[s].add(str_k)
dynamic_poses[str_k] = np.array(
pose_expr.subs(
{s: 0
for s in pose_expr.free_symbols}).tolist())
else:
#print('{} is static, setting its pose to:\n{}'.format(k, pose_expr))
static_objects.append(self._collision_objects[str_k])
static_poses.append(np.array(pose_expr.tolist()))
else:
self._process_link_removal(k)
if len(static_objects) > 0:
pb.batch_set_transforms(static_objects, np.vstack(static_poses))
self._static_objects = static_objects
# print('\n '.join(['{}: {}'.format(n, c.transform) for n, c in self._collision_objects.items()]))
if len(dynamic_poses) > 0:
objs, matrices = zip(*[(self._collision_objects[k], m)
for k, m in dynamic_poses.items()])
pb.batch_set_transforms(objs, np.vstack(matrices))
super(GeometryModel, self).dispatch_events()
def __init__(self,
robot_prefix,
joint_topic,
joint_vel_symbols,
base_topic=None,
base_symbols=None,
base_watchdog=rospy.Duration(0.4),
reference_frame='odom'):
self.control_alias = {
s: str(Path(gm.erase_type(s))[-1])
for s in joint_vel_symbols
}
self._robot_cmd_msg = JointStateMsg()
self._robot_cmd_msg.name = list(self.control_alias.keys())
self._state_cb = None
self.robot_prefix = robot_prefix
if base_symbols is not None:
self.base_aliases = base_symbols
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
self.reference_frame = reference_frame
self._base_cmd_msg = TwistMsg()
self.pub_base_command = rospy.Publisher(base_topic,
TwistMsg,
queue_size=1,
tcp_nodelay=True)
self._base_last_cmd_stamp = None
self._base_cmd_lock = RLock()
self._base_thread = threading.Thread(
target=self._base_cmd_repeater, args=(base_watchdog, ))
self._base_thread.start()
else:
self.base_aliases = None
self.pub_joint_command = rospy.Publisher(joint_topic,
JointStateMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_robot_js = rospy.Subscriber('/joint_states',
JointStateMsg,
callback=self.cb_robot_js,
queue_size=1)
def set_data(self, key, value):
if type(key) is str:
key = Path(key)
if isinstance(value, RigidBody):
self._process_body_insertion(key, value)
elif isinstance(value, KinematicJoint):
self._process_joint_insertion(key, value)
elif isinstance(value, ArticulatedObject):
for lname, link in value.links.items():
self._process_body_insertion(key + (
'links',
lname,
), link)
for jname, joint in value.joints.items():
self._process_joint_insertion(key + (
'joints',
jname,
), joint)
super(GeometryModel, self).set_data(key, value)
def get_data(self, key):
if type(key) == str:
key = Path(key)
return self.data_tree[key]
def behavior_update(self):
state_count = 0
while not rospy.is_shutdown() and not self._kys:
loop_start = rospy.Time.now()
with self._state_lock:
if self._robot_state_update_count <= state_count:
rospy.sleep(0.01)
continue
state_count = self._robot_state_update_count
if self.controller is not None:
now = rospy.Time.now()
with self._state_lock:
deltaT = 0.05 if self._last_controller_update is None else (
now - self._last_controller_update).to_sec()
try:
command = self.controller.get_cmd(self._state,
deltaT=deltaT)
except Exception as e:
print(traceback.format_exc())
rospy.signal_shutdown('die lol')
self.robot_command_processor.send_command(command)
self._last_controller_update = now
# Lets not confuse the tracker
if self._phase != 'opening' and self._last_external_cmd_msg is not None:
# Ensure that velocities are assumed to be 0 when not operating anything
self._last_external_cmd_msg.value = [0] * len(
self._last_external_cmd_msg.value)
self.pub_external_command.publish(self._last_external_cmd_msg)
self._last_external_cmd_msg = None
if self._phase == 'idle':
# if there is no controller, instantiate idle
# check for new target
# -> open gripper
# instantiate 6d controller
# switch to "grasping"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
with self._state_lock:
for s, p in self._target_map.items():
if s in self._state and self._state[
s] < self._var_upper_bound[
s] * self.monitoring_threshold: # Some thing in the scene is closed
self._current_target = p
print(f'New target is {self._current_target}')
self.gripper_wrapper.sync_set_gripper_position(
0.07)
self._last_controller_update = None
print('Gripper is open. Proceeding to grasp...')
draw_fn = None
eef_pose = self.km.get_data(self.eef_path).pose
if self.visualizer is not None:
self.visualizer.begin_draw_cycle('grasp pose')
self.visualizer.draw_poses(
'grasp pose', np.eye(4), 0.2, 0.01, [
gm.subs(self._grasp_poses[p],
self._state)
])
self.visualizer.render('grasp pose')
def draw_fn(state):
self.visualizer.begin_draw_cycle(
'debug_grasp')
self.visualizer.draw_poses(
'debug_grasp', np.eye(4), 1.0, 0.01, [
gm.subs(eef_pose, state),
gm.subs(self._grasp_poses[p],
state)
])
self.visualizer.render('debug_grasp')
# print('\n'.join(f'{s}: {v}' for s, v in state.items()))
self.controller = SixDPoseController(
self.km, eef_pose, self._grasp_poses[p],
self.controlled_symbols, self.weights, draw_fn)
self._phase = 'grasping'
print(f'Now entering {self._phase} state')
break
else:
print(
'None of the monitored symbols are closed:\n {}'.
format('\n '.join(
f'{self._state[s]} < {self._var_upper_bound[s]}'
for s in self._target_map.keys()
if s in self._state)))
elif self._phase == 'grasping':
# check if grasp is acheived
# -> close gripper
# instantiate cascading controller
# switch to "opening"
# if there is no more command but the goal error is too great -> "homing"
if self.controller.equilibrium_reached(0.03, -0.03):
if self.controller.current_error() > 0.01:
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
else:
print('Closing gripper...')
self.gripper_wrapper.sync_set_gripper_position(0, 80)
print('Generating opening controller')
eef = self.km.get_data(self.eef_path)
obj = self.km.get_data(self._current_target)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
static_object_pose = gm.subs(obj.pose, self._state)
offset_pose = np_inverse_frame(static_object_pose).dot(
static_eef_pose)
print(offset_pose)
goal_pose = gm.dot(obj.pose, gm.Matrix(offset_pose))
goal_pos_error = gm.norm(
gm.pos_of(eef.pose) - gm.pos_of(goal_pose))
goal_rot_error = gm.norm(eef.pose[:3, :3] -
goal_pose[:3, :3])
target_symbols = self._target_body_map[
self._current_target]
lead_goal_constraints = {
f'open_{s}': SC(self._var_upper_bound[s] - s, 1000,
1, s)
for s in target_symbols
if s in self._var_upper_bound
}
for n, c in lead_goal_constraints.items():
print(f'{n}: {c}')
follower_goal_constraints = {
'keep position':
SC(-goal_pos_error, -goal_pos_error, 10,
goal_pos_error),
'keep rotation':
SC(-goal_rot_error, -goal_rot_error, 1,
goal_rot_error)
}
blacklist = {
gm.Velocity(Path('pr2/torso_lift_joint'))
}.union({
gm.DiffSymbol(s)
for s in gm.free_symbols(obj.pose)
if 'location' in str(s)
})
# blacklist = {gm.DiffSymbol(s) for s in gm.free_symbols(obj.pose) if 'location' in str(s)}
self.controller = CascadingQP(
self.km,
lead_goal_constraints,
follower_goal_constraints,
f_gen_follower_cvs=gen_dv_cvs,
controls_blacklist=blacklist,
t_follower=GQPB,
visualizer=None # self.visualizer
)
print(self.controller)
# def debug_draw(vis, state, cmd):
# vis.begin_draw_cycle('lol')
# vis.draw_poses('lol', np.eye(4), 0.2, 0.01,
# [gm.subs(goal_pose, state),
# gm.subs(eef.pose, state)])
# vis.render('lol')
# self.controller.follower_qp._cb_draw = debug_draw
# self.gripper_wrapper.sync_set_gripper_position(0.07)
# rospy.signal_shutdown('die lol')
# return
self._last_controller_update = None
self._phase = 'opening'
print(f'Now entering {self._phase} state')
elif self._phase == 'opening':
# Wait for monitored symbols to be in open position
# -> open gripper
# generate 6d retraction goal: -10cm in tool frame
# spawn 6d controller
# switch to "retracting"
# self.visualizer.begin_draw_cycle('world state')
# with self._state_lock:
# self._world.update_world(self._state)
# self.visualizer.draw_world('world state', self._world, b=0)
# self.visualizer.render('world state')
external_command = {
s: v
for s, v in command.items()
if s not in self.controlled_symbols
}
ext_msg = ValueMapMsg()
ext_msg.header.stamp = now
ext_msg.symbol, ext_msg.value = zip(
*[(str(s), v) for s, v in external_command.items()])
self.pub_external_command.publish(ext_msg)
self._last_external_cmd_msg = ext_msg
with self._state_lock:
current_external_error = {
s: self._state[s]
for s in self._target_body_map[self._current_target]
}
print('Current error state:\n {}'.format('\n '.join(
[f'{s}: {v}' for s, v in current_external_error.items()])))
gripper_err = self.gripper_wrapper.get_latest_error()
# if gripper_err < 0.0005: # Grasped object is thinner than 5mm aka we lost it
# self.controller = self._idle_controller
# self._phase = 'homing'
# print(f'Grasped object seems to have slipped ({gripper_err}). Returning to home...')
# return
if min([
v >=
self._var_upper_bound[s] * self.acceptance_threshold
for s, v in current_external_error.items()
]):
print('Target fulfilled. Setting it to None')
self._current_target = None
eef = self.km.get_data(self.eef_path)
with self._state_lock:
static_eef_pose = gm.subs(eef.pose, self._state)
goal_pose = static_eef_pose.dot(
np.array([[1, 0, 0, -0.12], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
self.controller = SixDPoseController(
self.km, eef.pose, goal_pose, self.controlled_symbols,
self.weights)
self.gripper_wrapper.sync_set_gripper_position(0.07)
self._last_controller_update = None
self._phase = 'retracting'
print(f'Now entering {self._phase} state')
else:
remainder = (1 / 3) - (rospy.Time.now() -
loop_start).to_sec()
if remainder > 0:
rospy.sleep(remainder)
elif self._phase == 'retracting':
# Wait for retraction to complete (Currently just skipping)
# -> spawn idle controller
# switch to "homing"
if self.controller.equilibrium_reached(0.035, -0.035):
self.controller = self._idle_controller
self._phase = 'homing'
print(f'Now entering {self._phase} state')
elif self._phase == 'homing':
# Wait for idle controller to have somewhat low error
# -> switch to "idle"
if self.controller is None:
self.gripper_wrapper.sync_set_gripper_position(0.07)
self.controller = self._idle_controller
continue
if self.controller.equilibrium_reached(0.1, -0.1):
self._phase = 'idle'
print(f'Now entering {self._phase} state')
else:
raise Exception(f'Unknown state "{self._phase}')
'PR2 will be loaded from parameter server. It is currently not there.'
)
exit(1)
urdf_model = load_urdf_file(
'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml')
# urdf_model = load_urdf_str(rospy.get_param('/robot_description'))
if urdf_model.name.lower() != 'pr2':
print(
f'The loaded robot is not the PR2. Its name is "{urdf_model.name}"'
)
exit(1)
km = GeometryModel()
load_urdf(km, Path('pr2'), urdf_model)
km.clean_structure()
reference_frame = rospy.get_param('~reference_frame',
urdf_model.get_root())
use_base = reference_frame != urdf_model.get_root()
if use_base:
insert_omni_base(km, Path('pr2'), urdf_model.get_root(),
reference_frame)
base_joint_path = Path(f'pr2/joints/to_{reference_frame}')
visualizer = ROSBPBVisualizer('~vis', base_frame=reference_frame)
model_name = load_localized_model(km, model_path, reference_frame)
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)
far = rospy.get_param('~far', 7.0)
fov = rospy.get_param('~fov', 60) * (np.pi / 180)
right = np.tan(0.5 * fov) * near
def create_nobilia_shelf(km,
prefix,
origin_pose=gm.eye(4),
parent_path=Path('world')):
km.apply_operation(
f'create {prefix}',
ExecFunction(prefix, MarkedArticulatedObject, str(prefix)))
shelf_height = 0.72
shelf_width = 0.6
shelf_body_depth = 0.35
wall_width = 0.016
l_prefix = prefix + ('links', )
geom_body_wall_l = Box(
l_prefix + ('body', ),
gm.translation3(0, 0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_wall_r = Box(
l_prefix + ('body', ),
gm.translation3(0, -0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_ceiling = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, 0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_floor = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, -0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_shelf_1 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, -0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_shelf_2 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, 0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_back = Box(
l_prefix + ('body', ),
gm.translation3(0.5 * (shelf_body_depth - 0.005), 0, 0),
gm.vector3(0.005, shelf_width - 2 * wall_width,
shelf_height - 2 * wall_width))
shelf_geom = [
geom_body_wall_l, geom_body_wall_r, geom_body_ceiling, geom_body_floor,
geom_body_back, geom_body_shelf_1, geom_body_shelf_2
]
rb_body = RigidBody(parent_path,
origin_pose,
geometry=dict(enumerate(shelf_geom)),
collision=dict(enumerate(shelf_geom)))
geom_panel_top = Box(l_prefix + ('panel_top', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
geom_panel_bottom = Box(l_prefix + ('panel_bottom', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
handle_width = 0.16
handle_depth = 0.05
handle_diameter = 0.012
geom_handle_r = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_l = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
-0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_bar = Box(
l_prefix + ('handle', ),
gm.translation3(handle_depth - 0.5 * handle_diameter, 0, 0),
gm.vector3(handle_diameter, handle_width - handle_diameter,
handle_diameter))
handle_geom = [geom_handle_l, geom_handle_r, geom_handle_bar]
# Sketch of mechanism
#
# T ---- a
# ---- \ Z
# b ..... V \
# | ...... d
# B | ------
# c ------
# L
#
# Diagonal V is virtual
#
#
# Angles:
# a -> alpha (given)
# b -> gamma_1 + gamma_2 = gamma
# c -> don't care
# d -> delta_1 + delta_2 = delta
#
opening_position = gm.Position(prefix + ('door', ))
# Calibration results
#
# Solution top hinge: cost = 0.03709980624159568 [ 0.08762252 -0.01433833 0.2858676 0.00871125]
# Solution bottom hinge: cost = 0.025004236048128934 [ 0.1072496 -0.01232362 0.27271013 0.00489996]
# Added 180 deg rotation due to -x being the forward facing side in this model
top_hinge_in_body_marker = gm.translation3(0.08762252 - 0.015, 0,
-0.01433833)
top_panel_marker_in_top_hinge = gm.translation3(0.2858676 - 0.003,
-wall_width + 0.0025,
0.00871125 - 0.003)
front_hinge_in_top_panel_maker = gm.translation3(0.1072496 - 0.02, 0,
-0.01232362 + 0.007)
bottom_panel_marker_in_front_hinge = gm.translation3(
0.27271013, 0, 0.00489996)
# Top hinge - Data taken from observation
body_marker_in_body = gm.dot(
gm.rotation3_axis_angle(gm.vector3(0, 0, 1), math.pi),
gm.translation3(0.5 * shelf_body_depth - 0.062,
-0.5 * shelf_width + 0.078, 0.5 * shelf_height))
top_panel_marker_in_top_panel = gm.translation3(
geom_panel_top.scale[0] * 0.5 - 0.062,
-geom_panel_top.scale[1] * 0.5 + 0.062, geom_panel_top.scale[2] * 0.5)
bottom_panel_marker_in_bottom_panel = gm.translation3(
geom_panel_bottom.scale[0] * 0.5 - 0.062,
-geom_panel_bottom.scale[1] * 0.5 + 0.062,
geom_panel_bottom.scale[2] * 0.5)
top_hinge_in_body = gm.dot(body_marker_in_body, top_hinge_in_body_marker)
top_panel_in_top_hinge = gm.dot(
top_panel_marker_in_top_hinge,
gm.inverse_frame(top_panel_marker_in_top_panel))
front_hinge_in_top_panel = gm.dot(top_panel_marker_in_top_panel,
front_hinge_in_top_panel_maker)
bottom_panel_in_front_hinge = gm.dot(
bottom_panel_marker_in_front_hinge,
gm.inverse_frame(bottom_panel_marker_in_bottom_panel))
# Point a in body reference frame
point_a = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(top_hinge_in_body))
point_d = gm.point3(-shelf_body_depth * 0.5 + 0.09, 0,
shelf_height * 0.5 - 0.192)
# point_d = gm.point3(-shelf_body_depth * 0.5 + gm.Symbol('point_d_x'), 0, shelf_height * 0.5 - gm.Symbol('point_d_z'))
# Zero alpha along the vertical axis
vec_a_to_d = gm.dot(point_d - point_a)
alpha = gm.atan2(vec_a_to_d[0], -vec_a_to_d[2]) + opening_position
top_panel_in_body = gm.dot(
top_hinge_in_body, # Translation hinge to body frame
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position +
0.5 * math.pi), # Hinge around y
top_panel_in_top_hinge)
front_hinge_in_body = gm.dot(top_panel_in_body, front_hinge_in_top_panel)
# Point b in top panel reference frame
point_b_in_top_hinge = gm.pos_of(
gm.dot(gm.diag(1, 0, 1, 1), front_hinge_in_top_panel,
top_panel_in_top_hinge))
point_b = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(front_hinge_in_body))
# Hinge lift arm in body reference frame
point_c_in_bottom_panel = gm.dot(
gm.diag(1, 0, 1, 1),
bottom_panel_marker_in_bottom_panel,
gm.point3(-0.094, -0.034, -0.072),
# gm.point3(-gm.Symbol('point_c_x'), -0.034, -gm.Symbol('point_c_z'))
)
point_c_in_front_hinge = gm.dot(
gm.diag(1, 0, 1, 1),
gm.dot(bottom_panel_in_front_hinge, point_c_in_bottom_panel))
length_z = gm.norm(point_a - point_d)
vec_a_to_b = point_b - point_a
length_t = gm.norm(vec_a_to_b)
length_b = gm.norm(point_c_in_front_hinge[:3])
# length_l = gm.Symbol('length_l') # 0.34
length_l = 0.372
vec_b_to_d = point_d - point_b
length_v = gm.norm(vec_b_to_d)
gamma_1 = inner_triangle_angle(length_t, length_v, length_z)
gamma_2 = inner_triangle_angle(length_b, length_v, length_l)
top_panel_offset_angle = gm.atan2(point_b_in_top_hinge[2],
point_b_in_top_hinge[0])
bottom_offset_angle = gm.atan2(point_c_in_front_hinge[2],
point_c_in_front_hinge[0])
gamma = gamma_1 + gamma_2
rb_panel_top = RigidBody(l_prefix + ('body', ),
gm.dot(rb_body.pose, top_panel_in_body),
top_panel_in_body,
geometry={0: geom_panel_top},
collision={0: geom_panel_top})
# old offset: 0.5 * geom_panel_top.scale[2] + 0.03
tf_bottom_panel = gm.dot(
front_hinge_in_top_panel,
gm.rotation3_axis_angle(
gm.vector3(0, 1, 0),
math.pi + bottom_offset_angle - top_panel_offset_angle),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), gamma),
bottom_panel_in_front_hinge)
rb_panel_bottom = RigidBody(l_prefix + ('panel_top', ),
gm.dot(rb_panel_top.pose, tf_bottom_panel),
tf_bottom_panel,
geometry={0: geom_panel_bottom},
collision={0: geom_panel_bottom})
handle_transform = gm.dot(
gm.translation3(geom_panel_bottom.scale[0] * 0.5 - 0.08, 0,
0.5 * wall_width),
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -math.pi * 0.5))
rb_handle = RigidBody(l_prefix + ('panel_bottom', ),
gm.dot(rb_panel_bottom.pose, handle_transform),
handle_transform,
geometry={x: g
for x, g in enumerate(handle_geom)},
collision={x: g
for x, g in enumerate(handle_geom)})
# Only debugging
point_c = gm.dot(rb_panel_bottom.pose, point_c_in_bottom_panel)
vec_b_to_c = point_c - point_b
km.apply_operation(f'create {prefix}/links/body',
CreateValue(rb_panel_top.parent, rb_body))
km.apply_operation(f'create {prefix}/links/panel_top',
CreateValue(rb_panel_bottom.parent, rb_panel_top))
km.apply_operation(
f'create {prefix}/links/panel_bottom',
CreateValue(l_prefix + ('panel_bottom', ), rb_panel_bottom))
km.apply_operation(f'create {prefix}/links/handle',
CreateValue(l_prefix + ('handle', ), rb_handle))
km.apply_operation(
f'create {prefix}/joints/hinge',
ExecFunction(
prefix + Path('joints/hinge'), RevoluteJoint,
CPath(rb_panel_top.parent), CPath(rb_panel_bottom.parent),
opening_position, gm.vector3(0, 1, 0), gm.eye(4), 0, 1.84, **{
f'{opening_position}':
Constraint(0 - opening_position, 1.84 - opening_position,
opening_position),
f'{gm.DiffSymbol(opening_position)}':
Constraint(-0.25, 0.25, gm.DiffSymbol(opening_position))
}))
m_prefix = prefix + ('markers', )
km.apply_operation(
f'create {prefix}/markers/body',
ExecFunction(m_prefix + ('body', ), Frame,
CPath(l_prefix + ('body', )),
gm.dot(rb_body.pose,
body_marker_in_body), body_marker_in_body))
km.apply_operation(
f'create {prefix}/markers/top_panel',
ExecFunction(m_prefix + ('top_panel', ), Frame,
CPath(l_prefix + ('panel_top', )),
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
top_panel_marker_in_top_panel))
km.apply_operation(
f'create {prefix}/markers/bottom_panel',
ExecFunction(
m_prefix + ('bottom_panel', ), Frame,
CPath(l_prefix + ('panel_bottom', )),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel),
bottom_panel_marker_in_bottom_panel))
return NobiliaDebug(
[
top_hinge_in_body,
gm.dot(
top_hinge_in_body,
gm.rotation3_axis_angle(gm.vector3(0, 1, 0),
-opening_position + 0.5 * math.pi),
top_panel_in_top_hinge, front_hinge_in_top_panel),
body_marker_in_body,
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel)
], [(point_a, vec_a_to_d), (point_a, vec_a_to_b),
(point_b, vec_b_to_d), (point_b, vec_b_to_c)],
{
'gamma_1':
gamma_1,
'gamma_1 check_dot':
gamma_1 - gm.acos(
gm.dot_product(-vec_a_to_b / gm.norm(vec_a_to_b),
vec_b_to_d / gm.norm(vec_b_to_d))),
'gamma_1 check_cos':
gamma_1 - inner_triangle_angle(
gm.norm(vec_a_to_b), gm.norm(vec_b_to_d), gm.norm(vec_a_to_d)),
'gamma_2':
gamma_2,
'gamma_2 check_dot':
gamma_2 - gm.acos(
gm.dot_product(vec_b_to_c / gm.norm(vec_b_to_c),
vec_b_to_d / gm.norm(vec_b_to_d))),
'length_v':
length_v,
'length_b':
length_b,
'length_l':
length_l,
'position':
opening_position,
'alpha':
alpha,
'dist c d':
gm.norm(point_d - point_c)
}, {
gm.Symbol('point_c_x'): 0.094,
gm.Symbol('point_c_z'): 0.072,
gm.Symbol('point_d_x'): 0.09,
gm.Symbol('point_d_z'): 0.192,
gm.Symbol('length_l'): 0.372
})
robot_str = [
x for x in sys.argv[1:] if ':=' not in x
][0] if len([x for x in sys.argv[1:] if ':=' not in x]) > 0 else 'fetch'
if robot_str == 'fetch':
resting_pose = {
"shoulder_pan_joint": 1.32,
"shoulder_lift_joint": 1.40,
"upperarm_roll_joint": -0.2,
"elbow_flex_joint": 1.72,
"forearm_roll_joint": 0.0,
"wrist_flex_joint": 1.66,
"wrist_roll_joint": 0.0
}
closer = ObsessiveObjectCloser(Path('iai_oven_area'),
Path('fetch/links/gripper_link'),
Path('fetch/links/head_camera_link'),
'/tracked/state',
'/fetch',
resting_pose=resting_pose)
elif robot_str == 'pr2':
resting_pose = {
'l_elbow_flex_joint': -2.1213,
'l_shoulder_lift_joint': 1.2963,
'l_shoulder_pan_joint': 0.0,
'l_upper_arm_roll_joint': 0.0,
'l_forearm_roll_joint': 0.0,
'l_wrist_flex_joint': -1.05,
'r_elbow_flex_joint': -2.1213,
'r_shoulder_lift_joint': 1.2963,
kitchen_model = urdf_filler(
URDF.from_xml_string(hacky_urdf_parser_fix(urdf_kitchen_str)))
#
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)
# 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,
def create_door(km,
prefix,
height,
width,
frame_width=0.05,
to_world_tf=gm.eye(4)):
km.apply_operation(f'create {prefix}',
ExecFunction(prefix, ArticulatedObject, 'door'))
prefix = prefix + ('links', )
base_plate_geom = Box(prefix + ('frame', ), gm.translation3(0, 0, 0.015),
gm.vector3(0.2, width + 0.2, 0.03))
frame_pillar_l_geom = Box(
prefix + ('frame', ),
gm.translation3(0, 0.5 * (width + frame_width), 0.5 * height + 0.03),
gm.vector3(frame_width, frame_width, height))
frame_pillar_r_geom = Box(
prefix + ('frame', ),
gm.translation3(0, -0.5 * (width + frame_width), 0.5 * height + 0.03),
gm.vector3(frame_width, frame_width, height))
frame_bar_geom = Box(
prefix + ('frame', ),
gm.translation3(0, 0, height + 0.5 * frame_width + 0.03),
gm.vector3(frame_width, width + 2 * frame_width, frame_width))
frame_rb = RigidBody(Path('world'),
to_world_tf,
geometry={
1: base_plate_geom,
2: frame_pillar_l_geom,
3: frame_pillar_r_geom,
4: frame_bar_geom
},
collision={
1: base_plate_geom,
2: frame_pillar_l_geom,
3: frame_pillar_r_geom,
4: frame_bar_geom
})
door_geom1 = Box(prefix + ('door', ), gm.translation3(0.015, 0, 0),
gm.vector3(0.03, width, height))
door_geom2 = Box(prefix + ('door', ), gm.translation3(-0.005, 0, 0.01),
gm.vector3(0.01, width + 0.02, height + 0.01))
handle_bar_geom = Box(prefix + ('handle', ),
gm.translation3(-0.08, 0.06, 0),
gm.vector3(0.02, 0.12, 0.02))
handle_cylinder_geom = Cylinder(
prefix + ('handle', ),
gm.dot(gm.translation3(-0.04, 0, 0),
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), 0.5 * math.pi)),
0.02, 0.08)
door_rb = RigidBody(Path(f'{prefix}/frame'),
gm.translation3(0.0, 0.5 * -width - 0.01, 0),
geometry={
1: door_geom1,
2: door_geom2
},
collision={
1: door_geom1,
2: door_geom2
})
handle_rb = RigidBody(Path(f'{prefix}/door'),
gm.eye(4),
geometry={
1: handle_bar_geom,
2: handle_cylinder_geom
},
collision={
1: handle_bar_geom,
2: handle_cylinder_geom
})
km.apply_operation(f'create {prefix}/frame',
CreateValue(prefix + ('frame', ), frame_rb))
km.apply_operation(f'create {prefix}/door',
CreateValue(prefix + ('door', ), door_rb))
km.apply_operation(f'create {prefix}/handle',
CreateValue(prefix + ('handle', ), handle_rb))
door_position = gm.Position('door')
handle_position = gm.Position('handle')
prefix = prefix[:-1] + ('joints', )
km.apply_operation(
f'create {prefix}',
ExecFunction(
prefix + ('hinge', ), RevoluteJoint, CPath(door_rb.parent),
CPath(handle_rb.parent), door_position, gm.vector3(0, 0, -1),
gm.translation3(0.5 * -frame_width - 0.005, 0.5 * width + 0.01,
0.5 * height + 0.03), 0, 0.75 * math.pi, 100, 1,
0))
km.apply_operation(
f'create {prefix}',
ExecFunction(prefix + ('handle', ), RevoluteJoint,
CPath(handle_rb.parent), CPath(f'{prefix}/handle'),
handle_position, gm.vector3(-1, 0, 0),
gm.translation3(0, -0.5 * width - 0.02 + 0.06,
0), 0, 0.25 * math.pi, 100, 1, 0))
prefix = prefix[:-1]
km.apply_operation(
f'connect {prefix}/links/frame {prefix}/links/door',
CreateURDFFrameConnection(prefix + ('joints', 'hinge'),
Path(door_rb.parent),
Path(handle_rb.parent)))
km.apply_operation(
f'connect {prefix}/links/door {prefix}/links/handle',
CreateURDFFrameConnection(prefix + ('joints', 'handle'),
Path(handle_rb.parent),
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))
def create_symbol_vec3(prefix):
if not isinstance(prefix, Path):
prefix = Path(prefix)
symbols = [p.to_symbol() for p in [prefix + (x,) for x in 'xyz']]
return SymbolVector3(gm.vector3(*symbols), *symbols)
def remove_data(self, key):
if type(key) == str:
key = Path(key)
self.data_tree.remove_data(key)
def create_symbol_frame3_rpy(prefix):
if not isinstance(prefix, Path):
prefix = Path(prefix)
symbols = [p.to_symbol() for p in ([prefix + (x,) for x in 'xyz'] + [prefix + (f'r{r}',) for r in 'rpy'])]
return SymbolPose3RPY(gm.frame3_rpy(*symbols[-3:], gm.point3(*symbols[:3])), *symbols)
def set_data(self, key, value):
if type(key) == str:
key = Path(key)
self.data_tree[key] = value
