def generate_constraints(self):
soft_constraints = {f'{self.link}_vel_{x}': SC((gm.dot(self.input_iframe.pose, self.input_lin_vel.vec))[x],
(gm.dot(self.input_iframe.pose, self.input_lin_vel.vec))[x],
1,
gm.pos_of(self.fk_frame)[x]) for x in range(3)}
self.goal_rotation = gm.dot(self.input_iframe.pose, self.input_rot_goal.pose, self.input_rot_offset.pose)
axis, angle = gm.axis_angle_from_matrix(gm.dot(gm.rot_of(self.fk_frame).T, self.goal_rotation))
r_rot_control = axis * angle
hack = gm.rotation3_axis_angle([0, 0, 1], 0.0001)
axis, angle = gm.axis_angle_from_matrix(gm.dot(gm.rot_of(self.fk_frame), hack))
c_aa = (axis * angle)
soft_constraints[f'{self.link} align rotation 0'] = SC(lower=r_rot_control[0],
upper=r_rot_control[0],
weight=1,
expr=c_aa[0])
soft_constraints[f'{self.link} align rotation 1'] = SC(lower=r_rot_control[1],
upper=r_rot_control[1],
weight=1,
expr=c_aa[1])
soft_constraints[f'{self.link} align rotation 2'] = SC(lower=r_rot_control[2],
upper=r_rot_control[2],
weight=1,
expr=c_aa[2])
return soft_constraints
def _build_ext_symbol_map(self, km, ext_paths):
# total_ext_symbols = set()
# Map of path to set of symbols
self._target_body_map = {}
self._grasp_poses = {}
self._var_upper_bound = {}
for path, grasp_pose in ext_paths.items():
data = km.get_data(path)
if not isinstance(data, Frame):
print(f'Given external path "{path}" is not a frame.')
continue
self._grasp_poses[path] = gm.dot(data.pose, grasp_pose)
symbols = {
s
for s in gm.free_symbols(data.pose) if 'location' not in str(s)
}
static_bounded_vars, \
static_bounds, \
_ = static_var_bounds(km, symbols)
print(static_bounds.shape)
if len(static_bounds) == 0:
constraints = km.get_constraints_by_symbols(symbols)
raise Exception(
'None of {} seem to have static bounds. Constraints are:\n '
.format(
', '.join(str(s) for s in symbols),
'\n '.join(f'{n} : {c}'
for n, c in constraints.items())))
self._var_upper_bound.update(
zip(static_bounded_vars, static_bounds[:, 1]))
# total_ext_symbols.update(gm.free_symbols(data.pose))
self._target_body_map[path] = set(static_bounded_vars)
list_sets = list(self._target_body_map.values())
for x, s in enumerate(list_sets):
# Only keep symbols unique to one target
s.difference_update(list_sets[:x] + list_sets[x + 1:])
for p, symbols in self._target_body_map.items():
for s in symbols:
self._target_map[s] = p
print('Monitored symbols are:\n {}'.format('\n '.join(
str(s) for s in self._target_map.keys())))
'upperarm_roll_joint': -1.57,
'wrist_flex_joint' : 1.66,
'shoulder_lift_joint': 1.4,
'torso_lift_joint' : 0.2}
start_pose = {gm.Position(Path(f'{robot_path}/{n}')): v for n, v in start_pose.items()}
else:
joint_symbols, \
robot_controlled_symbols, \
start_pose, \
eef, \
blacklist = generic_setup(km, robot_path, rospy.get_param('~eef', 'gripper_link'))
collision_world = km.get_active_geometry(gm.free_symbols(handle.pose).union(gm.free_symbols(eef.pose)))
# Init step
grasp_in_handle = gm.dot(gm.translation3(0.04, 0, 0), gm.rotation3_rpy(math.pi * 0.5, 0, math.pi))
goal_pose = gm.dot(handle.pose, grasp_in_handle)
goal_0_pose = gm.subs(goal_pose, {s: 0 for s in gm.free_symbols(goal_pose)})
start_state = {s: 0 for s in gm.free_symbols(collision_world)}
start_state.update(start_pose)
ik_err, robot_start_state = ik_solve_one_shot(km, eef.pose, start_state, goal_0_pose)
start_state.update({s: 0 for s in gm.free_symbols(handle.pose)})
start_state.update(robot_start_state)
collision_world.update_world(start_state)
vis.begin_draw_cycle('ik_solution')
vis.draw_world('ik_solution', collision_world)
vis.render('ik_solution')
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}')
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
})
def __init__(self,
km,
actuated_object_path,
target_object_path,
controlled_symbols,
start_state,
camera_path=None,
navigation_method='cross',
visualizer=None,
weight_override=None,
collision_avoidance_paths=None):
print(f'{actuated_object_path}\n{target_object_path}')
actuated_object = km.get_data(actuated_object_path)
target_object = km.get_data(target_object_path)
all_controlled_symbols = controlled_symbols.union({
gm.DiffSymbol(j)
for j in gm.free_symbols(target_object.pose)
if 'location' not in str(j)
})
static_symbols = {
s
for s in gm.free_symbols(target_object.pose)
if 'location' in str(s)
}
# Generate push problem
constraints, \
geom_distance, \
coll_world, \
self.p_internals = generate_push_closing(km,
start_state,
all_controlled_symbols,
actuated_object.pose,
target_object.pose,
actuated_object_path,
target_object_path,
navigation_method,
static_symbols=static_symbols)
start_state.update({s: 0.0 for s in gm.free_symbols(coll_world)})
weight_override = {} if weight_override is None else weight_override
controlled_values, \
constraints = generate_controlled_values(constraints,
all_controlled_symbols)
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.1)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.00)
}
for s, w in weight_override.items():
for cv in controlled_values.values():
if cv.symbol is s:
cv.weight_id = w
break
# CAMERA STUFF
if camera_path is not None:
camera = km.get_data(camera_path)
cam_pos = gm.pos_of(camera.pose)
cam_to_obj = gm.pos_of(target_object.pose) - cam_pos
cam_forward = gm.dot(camera.pose, gm.vector3(1, 0, 0))
look_goal = 1 - (gm.dot_product(cam_to_obj, cam_forward) /
gm.norm(cam_to_obj))
goal_constraints['look_at_obj'] = SC(-look_goal, -look_goal, 1,
look_goal)
# GOAL CONSTAINT GENERATION
# 'avoid_collisions': SC.from_constraint(closest_distance_constraint_world(eef_pose, eef_path[:-1], 0.03), 100)
# }
if collision_avoidance_paths is not None:
for p in collision_avoidance_paths:
obj = km.get_data(p)
goal_constraints[f'avoid_collision {p}'] = SC.from_constraint(
closest_distance_constraint(actuated_object.pose, obj.pose,
actuated_object_path, p, 0.01),
100)
goal_constraints.update({
f'open_object_{x}': PIDC(s, s, 1)
for x, s in enumerate(gm.free_symbols(target_object.pose))
})
self.look_goal = look_goal if camera_path is not None else None
self.in_contact = gm.less_than(geom_distance, 0.01)
self.controlled_values = controlled_values
self.geom_distance = geom_distance
self.qpb = GQPB(coll_world,
constraints,
goal_constraints,
controlled_values,
visualizer=visualizer)
self.qpb._cb_draw = self.p_internals.f_debug_draw
def generate_push_closing(km,
grounding_state,
controlled_symbols,
eef_pose,
obj_pose,
eef_path,
obj_path,
nav_method='cross',
cp_offset=0,
static_symbols=set()):
# CONTACT GEOMETRY
robot_cp, object_cp, contact_normal = contact_geometry(
eef_pose, obj_pose, eef_path, obj_path)
object_cp = object_cp - contact_normal * cp_offset
geom_distance = gm.dot_product(contact_normal, robot_cp - object_cp)
coll_world = km.get_active_geometry(gm.free_symbols(geom_distance))
# GEOMETRY NAVIGATION LOGIC
# This is exploiting task knowledge which makes this function inflexible.
contact_grad = sum([
sign(-grounding_state[s]) *
gm.vector3(gm.diff(object_cp[0], s), gm.diff(object_cp[1], s),
gm.diff(object_cp[2], s))
for s in gm.free_symbols(obj_pose) if s not in static_symbols
], gm.vector3(0, 0, 0))
neutral_tangent = gm.cross(contact_grad, contact_normal)
active_tangent = gm.cross(neutral_tangent, contact_normal)
contact_constraints, in_contact = generate_contact_model(
robot_cp, controlled_symbols, object_cp, contact_normal,
gm.free_symbols(obj_pose))
target_pos = None
if nav_method == 'linear':
geom_distance = gm.norm(object_cp + active_tangent * geom_distance +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cubic':
dist_scaling = 2**(-0.5 * ((geom_distance - 0.2) / (0.2 * 0.2))**2)
geom_distance = gm.norm(object_cp + active_tangent * dist_scaling -
robot_cp)
elif nav_method == 'cross':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cross_deep':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad *
-gm.dot_product(contact_normal, contact_grad) -
robot_cp)
elif nav_method == 'none' or nav_method is None:
pass
elif nav_method == 'proj':
obj_cp_dist = gm.dot_product(contact_normal,
object_cp - gm.pos_of(obj_pose))
target_pos = gm.pos_of(
obj_pose
) + contact_normal * obj_cp_dist - contact_normal * 0.02 # Drive into the surface
geom_distance = gm.norm(robot_cp - target_pos)
contact_relative_pos = gm.dot(gm.rot_of(obj_pose),
robot_cp - gm.pos_of(obj_pose))
contact_relative_vel = gm.vector3(
sum([gm.diff(contact_relative_pos[0], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[1], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[2], s) for s in controlled_symbols],
0))
# PUSH CONSTRAINT GENERATION
constraints = km.get_constraints_by_symbols(
gm.free_symbols(geom_distance).union(controlled_symbols))
constraints.update(contact_constraints)
# for x, n in enumerate('xyz'):
# constraints[f'zero tangent vel_{n}'] = Constraint((1 - in_contact) * -1e3,
# (1 - in_contact) * 1e3, contact_relative_pos[x] * (1.0 + 0.1 * x))
def debug_draw(vis, state, cmd):
vis.begin_draw_cycle('debug_vecs')
s_object_cp = gm.subs(object_cp, state)
s_neutral_tangent = gm.subs(neutral_tangent, state)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(contact_grad, state),
r=0,
b=0)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(active_tangent, state),
r=0,
b=1)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(neutral_tangent, state),
r=1,
g=1,
b=0)
if target_pos is not None:
vis.draw_sphere('debug_vecs',
gm.subs(target_pos, state),
0.01,
r=0,
b=1)
# print(f'{gm.norm(gm.subs(contact_normal, state))}')
# vis.draw_vector('debug_vecs', s_object_cp, s_ortho_vel_vec, r=1, b=0)
vis.render('debug_vecs')
return constraints, geom_distance, coll_world, PushingInternals(
robot_cp, contact_normal, contact_relative_pos, contact_relative_vel,
debug_draw)
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))
door = km.get_data('door')
handle = door.links['handle']
iiwa = km.get_data('iiwa')
symbols = gm.free_symbols(door.links['handle'].pose).union(
gm.free_symbols(iiwa.links['link_7'].pose))
door_position = door.joints['hinge'].position
handle_position = door.joints['handle'].position
# Fun for visuals
world = km.get_active_geometry(symbols)
symbols = world.free_symbols
eef = iiwa.links['wsg_50_tool_frame']
goal_pose = gm.dot(handle.pose, gm.translation3(-0.08, 0.1, 0),
gm.rotation3_rpy(0, math.pi * 0.5, 0))
ik_solver = IKSolver(km, eef.pose, visualizer)
# Working config is
# x: 0.8333333333333333
# y: -0.1333333333333333
for x_coord in [0.8]: #np.linspace(0.5, 1.0, 4):
for y_coord in [-0.1]: #np.linspace(0.2, -0.8, 4):
print(f'Config is ({x_coord}, {y_coord})')
ik_goal_start = {s: 0 for s in world.free_symbols}
ik_goal_start[door_x] = x_coord
ik_goal_start[door_y] = y_coord
err_ik, q_ik_goal = ik_solve_one_shot(km, eef.pose, ik_goal_start,
goal_pose)
fac = (stamp - p_data.stamp).to_sec() / segment_length
return (1 - fac) * getattr(p_data, value) + fac * getattr(data, value)
angle_hinge = gm.Symbol('angle_top')
x_hinge_in_parent, z_hinge_in_parent = [
gm.Symbol(f'hinge_in_parent_{x}') for x in 'xz'
]
x_child_in_hinge, z_child_in_hinge = [
gm.Symbol(f'child_in_hinge_{x}') for x in 'xz'
]
fwd_kinematic_hinge = gm.dot(
gm.translation3(x_hinge_in_parent, 0, z_hinge_in_parent),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), angle_hinge),
gm.translation3(x_child_in_hinge, 0, z_child_in_hinge))
# we don't care about the location in y
fwd_kinematic_hinge_residual_tf = gm.speed_up(
gm.dot(gm.diag(1, 0, 1, 1), fwd_kinematic_hinge),
gm.free_symbols(fwd_kinematic_hinge))
def compute_y_hinge_axis_residual(x, angle_tf):
"""Computes the residual for an estimate of the hinge locations on the nobilia shelf
Args:
x (np.ndarray): [xh, zh, xc, zc] x and z locations of hinge in parent and child in hinge
angle_1_tf (TYPE): list of tuples (a, tf) where a is the angle of the top panel relative to the parent
"""
param_dict = {
import kineverse.gradients.gradient_math as gm
import numpy as np
import matplotlib.pyplot as plt
from kineverse.visualization.plotting import hsv_to_rgb, \
rgb_to_hex
if __name__ == '__main__':
a, b = [gm.Position(x) for x in 'ab']
l = 2
a_in_w = gm.dot(gm.translation3(0, 0, 2), gm.translation3(0, 0, -a))
d_in_a = gm.rotation3_axis_angle(gm.vector3(0, 1, 0), gm.acos(a / l))
d_in_w = gm.dot(a_in_w, d_in_a)
A = gm.pos_of(a_in_w)
B = gm.dot(d_in_w, gm.point3(0, 0, l))
C = gm.dot(d_in_w, gm.point3(0, 0, l * 0.5))
D = gm.dot(d_in_w, gm.point3(0, 0, l * 0.25))
E = gm.dot(d_in_w, gm.point3(0, 0, l * 0.75))
lock_bound = gm.alg_not(
gm.alg_and(gm.less_than(b, 0.3), gm.less_than(1.99, a)))
# PLOT OF MOVEMENT
As = []
Bs = []
Cs = []
Ds = []
Es = []
config_delta = []
n_crashes = 0
for linear_std, angular_std in zip(
np.linspace(0, args.noise_lin, args.noise_steps),
np.linspace(0, args.noise_ang * (np.pi / 180.0),
args.noise_steps)):
for x in range(args.samples):
# Uniformly sample a joint state
joint_state = dict(
zip(joint_array,
np.random.rand(len(joint_array)) * scale + offset))
# Generate n noise transforms
noise = [
cm.to_numpy(dot(t, r)) for t, r in zip(
random_normal_translation(len(frames), 0, linear_std),
random_rot_normal(len(frames), 0, angular_std))
]
# Calculate forward kinematics of frames
obs_frames = {
k: subs(f, joint_state).dot(n)
for (k, f), n in zip(frames.items(), noise)
}
for x, (k, f) in enumerate(obs_frames.items()):
update_msg.poses[x].header.frame_id = k
update_msg.poses[x].pose.position.x = float(f[0, 3])
update_msg.poses[x].pose.position.y = float(f[1, 3])
update_msg.poses[x].pose.position.z = float(f[2, 3])