temp = state.copy()
temp.update(cmd)
state.update({s: subs(e, temp) for s, e in int_rules.items()})
diff_points.append((state[x1], state[y1], state[a1]))
# md_points.append((state[x2], state[y2], state[a2]))
# if qpb.equilibrium_reached():
# print('optimization ende early')
# break
full_diff_points.append(diff_points)
vis = ROSVisualizer('diff_drive_vis', 'world')
# md_p = [point3(x, y, 0) for x, y, _ in md_points]
# md_d = [vector3(cos(a), sin(a), 0) for _, _, a in md_points]
vis.begin_draw_cycle('paths')
# vis.draw_sphere('paths', goal, 0.02, r=0, b=1)
for n, diff_points in enumerate(full_diff_points):
diff_p = [point3(x, y, 0) + vector3((n / 3) * 0.5, (n % 3)* -0.5, 0) for x, y, _ in diff_points]
diff_d = [vector3(cos(a), sin(a), 0) for _, _, a in diff_points]
vis.draw_strip('paths', cm.eye(4), 0.02, diff_p)
#vis.draw_strip('paths', spw.eye(4), 0.02, md_p, r=0, g=1)
vis.render('paths')
rospy.sleep(0.3)
class PoseObservationPublisher(SimulatorPlugin):
def __init__(self, multibody, camera_link, fov, near, far, noise_exp, topic_prefix='', frequency=30, debug=False):
super(PoseObservationPublisher, self).__init__('PoseObservationPublisher')
self.topic_prefix = topic_prefix
self.publisher = rospy.Publisher('{}/pose_obs'.format(topic_prefix), PSAMsg, queue_size=1, tcp_nodelay=True)
self.message_templates = {}
self.multibody = multibody
self.camera_link = camera_link
self.fov = fov
self.near = near
self.far = far
self.noise_exp = noise_exp
self._enabled = True
self.visualizer = ROSVisualizer('pose_obs_viz', 'world') if debug else None
self._last_update = 1000
self._update_wait = 1.0 / frequency
def post_physics_update(self, simulator, deltaT):
"""Implements post physics step behavior.
:type simulator: BasicSimulator
:type deltaT: float
"""
if not self._enabled:
return
self._last_update += deltaT
if self._last_update >= self._update_wait:
self._last_update = 0
cf_tuple = self.multibody.get_link_state(self.camera_link).worldFrame
camera_frame = gm.frame3_quaternion(cf_tuple.position.x, cf_tuple.position.y, cf_tuple.position.z, *cf_tuple.quaternion)
cov_proj = gm.rot_of(camera_frame)[:3, :3]
inv_cov_proj = cov_proj.T
out = PSAMsg()
if self.visualizer is not None:
self.visualizer.begin_draw_cycle()
poses = []
for name, body in simulator.bodies.items():
if body == self.multibody:
continue
if isinstance(body, MultiBody):
poses_to_process = [('{}/{}'.format(name, l), body.get_link_state(l).worldFrame) for l in body.links]
else:
poses_to_process = [(name, body.pose())]
for pname, pose in poses_to_process:
if not pname in self.message_templates:
msg = PoseStampedMsg()
msg.header.frame_id = pname
self.message_templates[pname] = msg
else:
msg = self.message_templates[pname]
obj_pos = gm.point3(*pose.position)
c2o = obj_pos - gm.pos_of(camera_frame)
dist = gm.norm(c2o)
if dist < self.far and dist > self.near and gm.dot_product(c2o, gm.x_of(camera_frame)) > gm.cos(self.fov * 0.5) * dist:
noise = 2 ** (self.noise_exp * dist) - 1
(n_quat, ) = np_random_quat_normal(1, 0, noise)
(n_trans, ) = np_random_normal_offset(1, 0, noise)
n_pose = pb.Transform(pb.Quaternion(*pose.quaternion), pb.Vector3(*pose.position)) *\
pb.Transform(pb.Quaternion(*n_quat), pb.Vector3(*n_trans[:3]))
if self.visualizer is not None:
poses.append(transform_to_matrix(n_pose))
msg.pose.position.x = n_pose.origin.x
msg.pose.position.y = n_pose.origin.y
msg.pose.position.z = n_pose.origin.z
msg.pose.orientation.x = n_pose.rotation.x
msg.pose.orientation.y = n_pose.rotation.y
msg.pose.orientation.z = n_pose.rotation.z
msg.pose.orientation.w = n_pose.rotation.w
out.poses.append(msg)
self.publisher.publish(out)
if self.visualizer is not None:
self.visualizer.draw_poses('debug', gm.se.eye(4), 0.1, 0.02, poses)
self.visualizer.render()
def disable(self, simulator):
"""Stops the execution of this plugin.
:type simulator: BasicSimulator
"""
self._enabled = False
self.publisher.unregister()
def to_dict(self, simulator):
"""Serializes this plugin to a dictionary.
:type simulator: BasicSimulator
:rtype: dict
"""
return {'body': simulator.get_body_id(self.body.bId()),
'camera_link': self.camera_link,
'fov': self.fov,
'near': self.near,
'far': self.far,
'noise_exp': self.noise_exp,
'topic_prefix': self.topic_prefix}
@classmethod
def factory(cls, simulator, init_dict):
body = simulator.get_body(init_dict['body'])
if body is None:
raise Exception('Body "{}" does not exist in the context of the given simulation.'.format(init_dict['body']))
return cls(body,
init_dict['camera_link'],
init_dict['fov'],
init_dict['near'],
init_dict['far'],
init_dict['noise_exp'],
init_dict['topic_prefix'])
def reset(self, simulator):
pass
class ROSQPEManager(object):
def __init__(self,
tracker: Kineverse6DQPTracker,
model=None,
model_path=None,
reference_frame='world',
urdf_param='/qp_description_check',
update_freq=30,
observation_alias=None):
self.tracker = tracker
self.last_observation = 0
self.last_update = 0
self.reference_frame = reference_frame
self.observation_aliases = {o: o for o in tracker.observation_names}
if observation_alias is not None:
for path, alias in observation_alias.items():
if path in self.observation_aliases:
self.observation_aliases[alias] = path
self.str_controls = {str(s) for s in self.tracker.get_controls()}
self.vis = ROSVisualizer('~vis', reference_frame)
if model is not None:
self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
model, Path('ekf'))
else:
self.broadcaster = None
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
self.pub_state = rospy.Publisher('~state_estimate',
ValueMapMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_observations = rospy.Subscriber('~observations',
TransformStampedArrayMsg,
callback=self.cb_obs,
queue_size=1)
self.sub_controls = rospy.Subscriber('~controls',
ValueMapMsg,
callback=self.cb_controls,
queue_size=1)
self.timer = rospy.Timer(rospy.Duration(1 / update_freq),
self.cb_update)
def cb_obs(self, transform_stamped_array_msg):
ref_frames = {}
num_valid_obs = 0
last_observation = {}
for trans in transform_stamped_array_msg.transforms:
if trans.child_frame_id not in self.observation_aliases:
continue
matrix = np_frame3_quaternion(
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z, trans.transform.rotation.x,
trans.transform.rotation.y, trans.transform.rotation.z,
trans.transform.rotation.w)
if trans.header.frame_id != self.reference_frame:
if trans.header.frame_id not in ref_frames:
try:
ref_trans = self.tf_buffer.lookup_transform(
self.reference_frame, trans.header.frame_id,
rospy.Time(0))
np_ref_trans = np_frame3_quaternion(
ref_trans.transform.translation.x,
ref_trans.transform.translation.y,
ref_trans.transform.translation.z,
ref_trans.transform.rotation.x,
ref_trans.transform.rotation.y,
ref_trans.transform.rotation.z,
ref_trans.transform.rotation.w)
ref_frames[trans.header.frame_id] = np_ref_trans
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print(
f'Exception raised while looking up {trans.header.frame_id} -> {self.reference_frame}:\n{e}'
)
break
else:
np_ref_trans = ref_frames[trans.header.frame_id]
matrix = np_ref_trans.dot(matrix)
last_observation[self.observation_aliases[
trans.child_frame_id]] = matrix
num_valid_obs += 1
# self.last_observation[trans.child_frame_id] = np_6d_pose_feature(trans.transform.translation.x,
# trans.transform.translation.y,
# trans.transform.translation.z,
# trans.transform.rotation.x,
# trans.transform.rotation.y,
# trans.transform.rotation.z,
# trans.transform.rotation.w)
else:
try:
if num_valid_obs == 0:
return
self.tracker.process_observation(last_observation)
self.last_observation += 1
except QPSolverException as e:
print(
f'Solver crashed during observation update. Skipping observation... Error:\n{e}'
)
return
self.vis.begin_draw_cycle('observations')
# temp_poses = [gm.frame3_axis_angle(feature[3:] / np.sqrt(np.sum(feature[3:]**2)), np.sqrt(np.sum(feature[3:]**2)), feature[:3]) for feature in self.last_observation.values()]
self.vis.draw_poses('observations', np.eye(4), 0.2, 0.01,
last_observation.values())
# self.vis.draw_poses('observations', np.eye(4), 0.2, 0.01, temp_poses)
self.vis.render('observations')
def cb_controls(self, map_message):
self.last_control = {
gm.Symbol(str_symbol): v
for str_symbol, v in zip(map_message.symbol, map_message.value)
if str_symbol in self.str_controls
}
self.tracker.process_control(self.last_control)
def cb_update(self, *args):
if self.last_observation == self.last_update:
return
self.last_update = self.last_observation
est_state = self.tracker.get_estimated_state()
state_msg = ValueMapMsg()
state_msg.header.stamp = rospy.Time.now()
state_msg.symbol, state_msg.value = zip(*est_state.items())
state_msg.symbol = [str(s) for s in state_msg.symbol]
self.pub_state.publish(state_msg)
if self.broadcaster is not None:
self.broadcaster.update_state(est_state)
self.broadcaster.publish_state()
class TrackerNode(object):
def __init__(self,
js_topic,
obs_topic,
integration_factor=0.05,
iterations=30,
visualize=False,
use_timer=True):
self.km_client = GeometryModel() # ModelClient(GeometryModel)
self._js_msg = ValueMapMsg()
self._integration_factor = integration_factor
self._iterations = iterations
self._use_timer = use_timer
self._unintialized_poses = set()
self.aliases = {}
self.tracked_poses = {}
self.integrator = None
self.lock = RLock()
self.soft_constraints = {}
self.joints = set()
self.joint_aliases = {}
self.visualizer = ROSVisualizer('/tracker_vis',
'world') if visualize else None
self.pub_js = rospy.Publisher(js_topic, ValueMapMsg, queue_size=1)
self.sub_obs = rospy.Subscriber(obs_topic,
PSAMsg,
self.cb_process_obs,
queue_size=5)
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 stop_tracking(self, model_path):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
if model_path in self.tracked_poses:
te = self.tracked_poses[model_path]
self.km_client.deregister_on_model_changed(te.model_cb)
del self.tracked_poses[model_path]
del self.soft_constraints['{} align rotation 0'.format(
model_path)]
del self.soft_constraints['{} align rotation 1'.format(
model_path)]
del self.soft_constraints['{} align rotation 2'.format(
model_path)]
del self.soft_constraints['{} align position'.format(
model_path)]
self.generate_opt_problem()
if len(self.tracked_poses) == 0:
self.timer.shutdown()
@profile
def cb_tick(self, timer_event):
with self.lock:
if self.integrator is not None:
self.integrator.run(self._integration_factor,
self._iterations,
logging=False)
self._js_msg.header.stamp = Time.now()
self._js_msg.symbol, self._js_msg.value = zip(
*[(n, self.integrator.state[s])
for s, n in self.joint_aliases.items()])
self.pub_js.publish(self._js_msg)
if self.visualizer is not None:
poses = [
t.pose.subs(self.integrator.state)
for t in self.tracked_poses.values()
]
self.visualizer.begin_draw_cycle('obs_points')
self.visualizer.draw_points(
'obs_points', cm.eye(4), 0.1,
[pos_of(p) for p in poses if len(p.free_symbols) == 0])
# self.visualizer.draw_poses('obs_points', se.eye(4), 0.1, 0.02, [p for p in poses if len(p.free_symbols) == 0])
self.visualizer.render('obs_points')
else:
print('Integrator does not exist')
@profile
def cb_process_obs(self, poses_msg):
# print('Got new observation')
with self.lock:
for pose_msg in poses_msg.poses:
if pose_msg.header.frame_id in self.aliases and self.integrator is not None:
self.tracked_poses[self.aliases[
pose_msg.header.frame_id]].update_state(
pose_msg.pose, self.integrator.state)
if self.aliases[pose_msg.header.
frame_id] in self._unintialized_poses:
self._unintialized_poses.remove(
self.aliases[pose_msg.header.frame_id])
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()
def generate_opt_problem(self):
joint_symbols = self.joints
opt_symbols = {DiffSymbol(j) for j in joint_symbols}
hard_constraints = self.km_client.get_constraints_by_symbols(
joint_symbols | opt_symbols)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, opt_symbols)
for mp in self._unintialized_poses:
state = self.integrator.state if self.integrator is not None else {}
te = self.tracked_poses[mp]
state.update({s: 0.0 for s in cm.free_symbols(te.pose)})
state = {} if self.integrator is None else self.integrator.state
self.integrator = CommandIntegrator(TQPB(hard_constraints,
self.soft_constraints,
controlled_values),
start_state=state)
self.integrator.restart('Pose Tracking')
return axis * (a / sin_a2)
if __name__ == '__main__':
rospy.init_node('axis_angle_vis')
vis = ROSVisualizer('axis_vis', 'world')
az, ay = [Position(x) for x in 'ax az'.split(' ')]
frame_rpy = frame3_rpy(0, ay, az, point3(0,0,0))
state = {ay: 0, az: 0}
points = [point3(0,0,0) + get_rot_vector(frame_rpy.subs({ay: sin(v), az: cos(v)})) for v in [(3.14512 / 25) * x for x in range(51)]]
vis.begin_draw_cycle('points')
vis.draw_strip('points', se.eye(4), 0.03, points)
vis.render('points')
rospy.sleep(1)
timer = Time()
while not rospy.is_shutdown():
now = Time.now()
if (now - timer).to_sec() >= 0.02:
state[ay] = sin(now.to_sec())
state[az] = cos(now.to_sec())
frame = frame_rpy.subs(state)
axis = get_rot_vector(frame)
class ROSEKFManager(object):
def __init__(self,
tracker: Kineverse6DEKFTracker,
model=None,
model_path=None,
urdf_param='/ekf_description_check'):
self.tracker = tracker
self.last_observation = {}
self.last_update = None
self.last_control = {s: 0.0 for s in self.tracker.controls}
self.str_controls = {str(s) for s in self.tracker.controls}
self.vis = ROSVisualizer('~vis', 'world')
if model is not None:
self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
model, Path('ekf'))
else:
self.broadcaster = None
self.pub_state = rospy.Publisher('~state_estimate',
ValueMapMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_observations = rospy.Subscriber('~observations',
TransformStampedArrayMsg,
callback=self.cb_obs,
queue_size=1)
self.sub_controls = rospy.Subscriber('~controls',
ValueMapMsg,
callback=self.cb_controls,
queue_size=1)
def cb_obs(self, transform_stamped_array_msg):
for trans in transform_stamped_array_msg.transforms:
matrix = np_frame3_quaternion(
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z, trans.transform.rotation.x,
trans.transform.rotation.y, trans.transform.rotation.z,
trans.transform.rotation.w)
self.last_observation[trans.child_frame_id] = matrix
# self.last_observation[trans.child_frame_id] = np_6d_pose_feature(trans.transform.translation.x,
# trans.transform.translation.y,
# trans.transform.translation.z,
# trans.transform.rotation.x,
# trans.transform.rotation.y,
# trans.transform.rotation.z,
# trans.transform.rotation.w)
self.vis.begin_draw_cycle('observations')
# temp_poses = [gm.frame3_axis_angle(feature[3:] / np.sqrt(np.sum(feature[3:]**2)), np.sqrt(np.sum(feature[3:]**2)), feature[:3]) for feature in self.last_observation.values()]
self.vis.draw_poses('observations', np.eye(4), 0.2, 0.01,
self.last_observation.values())
self.vis.render('observations')
self.try_update()
def cb_controls(self, map_message):
self.last_control = {
gm.Symbol(str_symbol): v
for str_symbol, v in zip(map_message.symbol, map_message.value)
if str_symbol in self.str_controls
}
self.try_update()
def try_update(self):
if min(p in self.last_observation
for p in self.tracker.observation_names) is False:
return
if len(self.tracker.controls) > 0 and min(
s in self.last_control
for s in self.tracker.controls) is False:
return
now = rospy.Time.now()
dt = 0.05 if self.last_update is None else (now -
self.last_update).to_sec()
self.tracker.process_update(self.last_observation, self.last_control,
dt)
est_state = self.tracker.get_estimated_state()
if len(est_state) == 0:
return
state_msg = ValueMapMsg()
state_msg.header.stamp = now
state_msg.symbol, state_msg.value = zip(*est_state.items())
state_msg.symbol = [str(s) for s in state_msg.symbol]
self.pub_state.publish(state_msg)
# print('Performed update of estimate and published it.')
# self.last_control = {}
self.last_observation = {}
if self.broadcaster is not None:
self.broadcaster.update_state(self.tracker.get_estimated_state())
self.broadcaster.publish_state()