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 __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 __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 __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)
for x in range(100): 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')
from kineverse.utils import real_quat_from_matrix def get_rot_vector(frame): qx, qy, qz, _ = real_quat_from_matrix(frame) axis = vector3(qx, qy, qz) sin_a2 = norm(axis) a = asin(sin_a2) * 2 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()
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()