def __init__(self, km,
actuated_pose,
goal_pose,
controlled_symbols,
weight_override=None,
draw_fn=None):
eef = actuated_pose
goal_pos_error = gm.norm(gm.pos_of(eef) - gm.pos_of(goal_pose))
self.eef_rot = eef[:3, :3]
self.goal_rot = goal_pose[:3, :3]
self.rot_error_matrix = self.eef_rot - self.goal_rot
goal_rot_error = gm.norm(self.rot_error_matrix)
rot_align_scale = gm.exp(-3 * goal_rot_error)
goal_constraints = {'align position': SC(-goal_pos_error * rot_align_scale,
-goal_pos_error * rot_align_scale, 10, goal_pos_error),
'align rotation': SC(-goal_rot_error, -goal_rot_error, 1, goal_rot_error)}
self.goal_rot_error = goal_rot_error
constraints = km.get_constraints_by_symbols(gm.free_symbols(eef).union(controlled_symbols))
cvs, constraints = generate_controlled_values(constraints, controlled_symbols, weights=weight_override)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=1.02)
self.draw_fn = draw_fn
self.qp = TQPB(constraints,
goal_constraints,
cvs)
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 rot_goal_non_hack(r_ctrl, r_goal):
axis, angle = axis_angle_from_matrix(dot(rot_of(r_ctrl).T, rot_of(r_goal)))
r_rot_control = axis * angle
r_dist = norm(r_rot_control)
return {'Align rotation no-hack': SC(-r_dist, -r_dist, 1, r_dist)}
def generate_push_controller(self):
if self.robot is None:
return
if self._current_target is None:
return
target_symbol = self.target_symbol_map[self._current_target]
pose_path = self._current_target[len(self.urdf_path):] + ('pose', )
obj_pose = pose_path.get_data(self.obj)
joint_symbols = self.robot_joint_symbols.union({target_symbol})
controlled_symbols = self.robot_controlled_symbols.union(
{DiffSymbol(target_symbol)})
# print('Generating push controller for symbols:\n {}'.format('\n '.join([str(s) for s in symbols])))
hard_constraints, geom_distance, self.geom_world, debug_draw = generate_push_closing(
self.km,
self.state,
controlled_symbols,
self.robot_eef.pose,
obj_pose,
self.robot_eef_path,
self._current_target,
'linear', # 'cross',
BULLET_FIXED_OFFSET)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(self.km,
controlled_values,
exp_factor=1.1)
if isinstance(self.base_joint, DiffDriveJoint):
controlled_values[str(self.base_joint.l_wheel_vel)].weight = 0.001
controlled_values[str(self.base_joint.r_wheel_vel)].weight = 0.001
print('\n'.join(sorted([str(s) for s in geom_distance.free_symbols])))
cam_to_obj = pos_of(obj_pose) - pos_of(self.robot_camera.pose)
lookat_dot = 1 - dot_product(self.robot_camera.pose * vector3(1, 0, 0),
cam_to_obj) / norm(cam_to_obj)
soft_constraints = {
'reach {}'.format(self._current_target):
PIDC(geom_distance, geom_distance, 1, k_i=0.02),
'close {}'.format(self._current_target):
PIDC(target_symbol, target_symbol, 1, k_i=0.06),
'lookat {}'.format(self._current_target):
SC(-lookat_dot, -lookat_dot, 1, lookat_dot)
}
self.soft_constraints = soft_constraints
print('Controlled Values:\n {}'.format('\n '.join(
[str(c) for c in controlled_values.values()])))
self.pushing_controller = GQPB(self.geom_world,
hard_constraints,
soft_constraints,
controlled_values,
visualizer=self.debug_visualizer)
def __init__(self, km, controlled_symbols, resting_pose, camera_path=None):
tucking_constraints = {}
if resting_pose is not None:
tucking_constraints = {
f'tuck {s}': SC(p - s, p - s, 1, s)
for s, p in resting_pose.items()
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
# tucking_constraints.update(self.taxi_constraints)
self.use_camera = camera_path is not None
if camera_path is not None:
self._poi_pos = gm.Symbol('poi')
poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3(
0, self._poi_pos * 2.0, 0)
camera = km.get_data(camera_path)
cam_to_poi = poi - gm.pos_of(camera.pose)
lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose),
cam_to_poi) / gm.norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= gm.free_symbols(c.expr)
joint_symbols = {
s
for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN
}
controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols}
hard_constraints = km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
km, controlled_values)
self.qp = TQPB(hard_constraints, tucking_constraints,
controlled_values)
self._start = Time.now()
def generate_idle_controller(self):
if self.robot is None:
return
tucking_constraints = {}
if self._resting_pose is not None:
tucking_constraints = {
'tuck {}'.format(s): SC(p - s, p - s, 1, s)
for s, p in self._resting_pose.items()
if s in self.robot_joint_symbols
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
tucking_constraints.update(self.taxi_constraints)
cam_to_poi = self.poi - pos_of(self.robot_camera.pose)
lookat_dot = 1 - dot_product(self.robot_camera.pose * vector3(1, 0, 0),
cam_to_poi) / norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= c.expr.free_symbols
joint_symbols = self.robot_joint_symbols.intersection(symbols)
controlled_symbols = self.robot_controlled_symbols
hard_constraints = self.km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
self.geom_world = self.km.get_active_geometry(joint_symbols,
include_static=True)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
self.km, controlled_values)
self.idle_controller = GQPB(self.geom_world,
hard_constraints,
tucking_constraints,
controlled_values,
visualizer=self.debug_visualizer)
def rot_goal_hack(r_ctrl, r_goal):
axis, angle = axis_angle_from_matrix(dot(rot_of(r_ctrl).T, rot_of(r_goal)))
r_rot_control = axis * angle
hack = rotation3_axis_angle([0, 0, 1], 0.0001)
axis, angle = axis_angle_from_matrix(dot(rot_of(r_ctrl).T, rot_of(r_ctrl), hack).T)
c_aa = (axis * angle)
r_dist = norm(r_rot_control - c_aa)
return {'Align rotation hack': SC(-r_dist, -r_dist, 1, r_dist)}
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.2)
print('Controlled values:\n{}'.format('\n'.join(
[str(x) for x in controlled_values.values()])))
print('Additional joint constraints:\n{}'.format('\n'.join([
str(c) for c in constraints.values()
if c.expr in controlled_symbols
])))
in_contact = less_than(geom_distance, 0.01)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.01),
'look_at_obj': SC(-look_goal, -look_goal, 1, look_goal)
}
goal_constraints.update({
'open_object_{}'.format(x): PIDC(s, s, 1)
for x, s in enumerate(obj_pose.free_symbols)
})
start_state = {s: 0.0 for s in coll_world.free_symbols}
start_state.update({s: 0.4 for s in obj_pose.free_symbols})
start_state.update({
Position(Path('fetch') + (k, )): v
for k, v in tucked_arm.items()
})
integrator = CommandIntegrator(
GQPB(coll_world,
def __init__(self, km, observations, transition_rules=None, max_iterations=20, num_samples=7):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
km (ArticulationModel): Articulation model to query for constraints
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.num_samples = num_samples
self.ordered_vars, \
self.transition_fn, \
self.transition_args = generate_transition_function(QPStateModel.DT_SYM,
state_vars,
transition_rules)
self.command_vars = {s for s in self.transition_args
if s not in state_vars and str(s) != str(QPStateModel.DT_SYM)}
obs_constraints = {}
obs_switch_vars = {}
# State as column vector n * 1
self.switch_vars = {}
self._obs_state = {}
self.obs_vars = {}
self.takers = {}
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
obs_switch_var = gm.Symbol(f'{o_label}_observed')
self.switch_vars[o_label] = obs_switch_var
if o_label not in obs_constraints:
obs_constraints[o_label] = {}
if o_label not in self.obs_vars:
self.obs_vars[o_label] = []
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
obs_symbol = gm.Symbol('{}_{}_{}'.format(o_label, *coords))
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - (1 - obs_switch_var) * 1e3,
-obs_error + (1 - obs_switch_var) * 1e3, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers[o_label] = indices
else:
obs_symbol = gm.Symbol(f'{o_label}_value')
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - obs_switch_var * 1e9,
-obs_error + obs_switch_var * 1e9, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
self.takers[o_label] = [0]
state_constraints = km.get_constraints_by_symbols(state_vars)
cvs, hard_constraints = generate_controlled_values(state_constraints,
{gm.DiffSymbol(s) for s in state_vars
if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN})
flat_obs_constraints = dict(sum([list(oc.items()) for oc in obs_constraints.values()], []))
self.qp = TQPB(hard_constraints, flat_obs_constraints, cvs)
st_bound_vars, st_bounds, st_unbounded = static_var_bounds(km, state_vars)
self._state = {s: 0 for s in st_unbounded} # np.random.uniform(-1.0, 1.0) for s in st_unbounded}
for vb, (lb, ub) in zip(st_bound_vars, st_bounds):
self._state[vb] = np.random.uniform(lb, ub)
self._state_buffer = []
self._state.update({s: 0 for s in self.transition_args})
self._obs_state = {s: 0 for s in sum(self.obs_vars.values(), [])}
self._obs_count = 0
self._stamp_last_integration = None
self._max_iterations = 10
self._current_error = 1e9
int_rules = {
x1: x1 + DT_SYM * (rw_v * cos(a1) * r * 0.5 + lw_v * cos(a1) * r * 0.5),
y1: y1 + DT_SYM * (rw_v * sin(a1) * r * 0.5 + lw_v * sin(a1) * r * 0.5),
a1: a1 + DT_SYM * (rw_v * (r / L) + lw_v * (- r / L)),
# x2: x2 + DT_SYM * l_v * cos(a2),
# y2: y2 + DT_SYM * l_v * sin(a2),
# a2: a2 + DT_SYM * a_v
}
goal = point3(1, 1, 0)
diff_dist = norm(dot(diff_drive, point3(0.1, 0, 0)) - goal) #
# md_dist = norm(dot(my_drive, point3(0.1, 0, 0)) - goal) #
qpb = TQPB({},
{'goal diff_drive': SC(-diff_dist, -diff_dist, 1, diff_dist)},
# 'goal my_drive': SC(-md_dist, -md_dist, 1, md_dist)},
{str(rw_v): CV(-r_limit, r_limit, rw_v, 0.001),
str(lw_v): CV(-r_limit, r_limit, lw_v, 0.001),
# str(l_v): CV(-1, 1, l_v, 0.001),
# str(a_v): CV(-a_limit, a_limit, a_v, 0.001)
})
full_diff_points = []
md_points = []
cmds = [
{rw_v: -0.8, lw_v: -0.8},
{rw_v: -1.0, lw_v: -0.0},
vis.render('ik_solution')
print(f'IK error: {ik_err}')
def gen_dv_cvs(km, constraints, controlled_symbols):
cvs, constraints = generate_controlled_values(constraints, controlled_symbols)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=1.02)
print('\n'.join(f'{cv.symbol}: {cv.weight_id}' for _, cv in sorted(cvs.items())))
return cvs, constraints
dyn_goal_pos_error = gm.norm(gm.pos_of(eef.pose) - gm.pos_of(goal_pose))
dyn_goal_rot_error = gm.norm(eef.pose[:3, :3] - goal_pose[:3, :3])
o_vars, bounds, _ = static_var_bounds(km, gm.free_symbols(handle.pose))
lead_goal_constraints = {f'open_object {s}': SC(ub - s,
ub - s, 1, s) for s, (_, ub) in zip(o_vars, bounds)}
follower_goal_constraints = {'keep position': SC(-dyn_goal_pos_error,
-dyn_goal_pos_error, 10, dyn_goal_pos_error),
'keep rotation': SC(-dyn_goal_rot_error, -dyn_goal_rot_error, 1, dyn_goal_rot_error)}
solver = CascadingQP(km,
lead_goal_constraints,
follower_goal_constraints,
f_gen_follower_cvs=gen_dv_cvs,
controls_blacklist=blacklist,
transition_overrides=integration_rules
)
# exit()
t_symbols, t_function, t_params = generate_transition_function(sym_dt, solver.state_symbols, overrides=integration_rules)
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 __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
ControlledValue as CV
from kineverse.motion.integrator import CommandIntegrator
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,
def _generate_pose_constraints(self, str_path, model):
with self.lock:
if str_path in self.tracked_poses:
align_rotation = '{} align rotation'.format(str_path)
align_position = '{} align position'.format(str_path)
if model is not None:
m_free_symbols = cm.free_symbols(model.pose)
if len(m_free_symbols) > 0:
te = self.tracked_poses[str_path]
self.joints |= m_free_symbols
self.joint_aliases = {s: str(s) for s in self.joints}
r_dist = norm(
cm.rot_of(model.pose) - cm.rot_of(te.pose))
self.soft_constraints[align_rotation] = SC(
-r_dist, -r_dist, 1, r_dist)
# print(align_position)
# print(model.pose)
dist = norm(cm.pos_of(model.pose) - cm.pos_of(te.pose))
# print('Distance expression:\n{}'.format(dist))
# print('Distance expression symbol overlap:\n{}'.format(m_free_symbols.intersection(cm.free_symbols(dist))))
# for s in m_free_symbols:
# print('Diff w.r.t {}:\n{}'.format(s, cm.diff(dist, s)))
self.soft_constraints[align_position] = SC(
-dist, -dist, 1, dist)
self.generate_opt_problem()
# Avoid crashes due to insufficient perception data. This is not fully correct.
if str_path in self._unintialized_poses:
state = self.integrator.state.copy(
) if self.integrator is not None else {}
state.update({
s: 0.0
for s in m_free_symbols if s not in state
})
null_pose = cm.subs(model.pose, state)
pos = cm.pos_of(null_pose)
quat = real_quat_from_matrix(cm.rot_of(null_pose))
msg = PoseMsg()
msg.position.x = pos[0]
msg.position.y = pos[1]
msg.position.z = pos[2]
msg.orientation.x = quat[0]
msg.orientation.y = quat[1]
msg.orientation.z = quat[2]
msg.orientation.w = quat[3]
te.update_state(msg, self.integrator.state)
else:
regenerate_problem = False
if align_position in self.soft_constraints:
del self.soft_constraints[align_position]
regenerate_problem = True
if align_rotation in self.soft_constraints:
del self.soft_constraints[align_rotation]
regenerate_problem = True
if regenerate_problem:
self.generate_opt_problem()
goal = 1.57 - door_position
controlled_symbols = {get_diff_symbol(button_position), door_velocity}
constraints = km.get_constraints_by_symbols(
controlled_symbols.union({button_position}))
print('----\n{}'.format('\n'.join([str(x) for x in constraints.values()])))
cvs, constraints = generate_controlled_values(constraints,
controlled_symbols)
print('Retrieved Constraints:\n{}'.format('\n'.join(
['{}:\n {}'.format(k, c) for k, c in constraints.items()])))
integrator = CommandIntegrator(TQPB(
constraints, {'goal': SC(-0.1, -0.1, 1, button_position)}, cvs),
start_state={button_position: 0},
recorded_terms={
'lock_lower': lock_constraint.lower,
'lock_upper': lock_constraint.upper,
'spring_lower': spring_constraint.lower,
})
integrator.restart('Door spring')
integrator.run(dt=0.05)
integrator.recorder.add_threshold('Door release', point_of_release, 'b')
plot_dir = res_pkg_path('package://kineverse/test/plots')
draw_recorders([integrator.recorder] +
split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,
def cb_robot_model_changed(self, model):
print('Robot model changed')
self.robot = model
temp = [j for j in self.robot.joints.values() if j.parent == 'world']
if len(temp) > 0:
self.base_joint = temp[0]
self.base_link = Path(
self.base_joint.child)[len(self.robot_eef_path) - 2:].get_data(
self.robot)
else:
self.base_link = [
l for l in self.robot.links.values() if l.parent == 'world'
][0]
self.robot_camera = self.robot_camera_path[len(self.robot_camera_path
) - 2:].get_data(
self.robot)
self.robot_eef = self.robot_eef_path[len(self.robot_eef_path) -
2:].get_data(self.robot)
self.robot_joint_symbols = {
j.position
for j in self.robot.joints.values()
if hasattr(j, 'position') and cm.is_symbol(j.position)
}
self.robot_controlled_symbols = {
DiffSymbol(j)
for j in self.robot_joint_symbols
}
if type(self.base_joint) is DiffDriveJoint:
self.robot_controlled_symbols.update(
{self.base_joint.l_wheel_vel, self.base_joint.r_wheel_vel})
self.robot_joint_symbols.update({
self.base_joint.x_pos, self.base_joint.y_pos,
self.base_joint.z_pos, self.base_joint.a_pos
})
self._needs_odom = True
elif type(self.base_joint) is OmnibaseJoint:
self.robot_controlled_symbols.update({
DiffSymbol(x)
for x in [
self.base_joint.x_pos, self.base_joint.y_pos,
self.base_joint.a_pos
]
})
self.robot_joint_symbols.update({
self.base_joint.x_pos, self.base_joint.y_pos,
self.base_joint.a_pos
})
self._needs_odom = True
new_state = {
s: 0.0
for s in self.robot_camera.pose.free_symbols.union(
self.robot_eef.pose.free_symbols)
if get_symbol_type(s) == TYPE_POSITION
}
new_state.update({s: 0.0 for s in self.robot_joint_symbols})
self.state.update(new_state)
common_prefix = self.robot_camera_path[:-2]
self.robot_js_aliases = {
str(Path(erase_type(s))[len(common_prefix):]): s
for s in self.robot_joint_symbols
}
self.inverse_js_aliases.update(
{erase_type(v): k
for k, v in self.robot_js_aliases.items()})
self.inverse_js_aliases.update({
erase_type(s): str(Path(erase_type(s))[len(common_prefix):])
for s in self.robot_controlled_symbols
})
print('Robot aliases:\n{}'.format('\n'.join([
'{:>20}: {}'.format(k, v)
for k, v in self.robot_js_aliases.items()
])))
# CREATE TAXI CONSTRAINTS
camera_o_z = x_of(self.robot_camera.pose)[2]
dist_start_location = norm(self.waiting_location -
pos_of(self.base_link.pose))
angular_alignment = dot_product(self.waiting_direction,
self.base_link.pose * vector3(1, 0, 0))
self.taxi_constraints = {
'to_position':
SC(-dist_start_location, -dist_start_location, 1,
dist_start_location),
'to_orientation':
SC(1 - angular_alignment - 2 * less_than(dist_start_location, 0.2),
1 - angular_alignment, 1, angular_alignment),
'camera_orientation':
SC(-0.2 - camera_o_z, -0.2 - camera_o_z, 1, camera_o_z)
}
self.generate_idle_controller()
def rot_goal_9D(r_ctrl, r_goal):
r_dist = norm((r_ctrl[:3, :3] - r_goal[:3, :3]).elements())
return {'Align rotation 9d': SC(-r_dist, -r_dist, 1, r_dist)}