Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef JOINTFILTER_H_
00033 #define JOINTFILTER_H_
00034
00035 #include <pcl/point_types.h>
00036 #include <unsupported/Eigen/MatrixFunctions>
00037 #include <Eigen/Geometry>
00038 #include <lgsm/Lgsm>
00039 #include <boost/shared_ptr.hpp>
00040 #include <boost/unordered_map.hpp>
00041
00042 #include <ros/ros.h>
00043 #include <geometry_msgs/Pose.h>
00044 #include <geometry_msgs/Twist.h>
00045 #include <geometry_msgs/PoseWithCovariance.h>
00046 #include <tf/tf.h>
00047 #include <tf/transform_datatypes.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <tf_conversions/tf_eigen.h>
00050
00051 #include <visualization_msgs/MarkerArray.h>
00052
00053 #include <wrappers/matrix/matrix_wrapper.h>
00054
00055 #include "omip_msgs/RigidBodyTwistWithCovMsg.h"
00056 #include "omip_common/OMIPTypeDefs.h"
00057
00058 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00059
00060 #define JOINT_AXIS_MARKER_RADIUS 0.01
00061 #define JOINT_AXIS_AND_VARIABLE_MARKER_RADIUS 0.02
00062 #define JOINT_VALUE_TEXT_SIZE 0.1
00063
00064
00065
00066 namespace omip
00067 {
00068
00069 typedef int long JointCombinedFilterId;
00070
00071 enum JointFilterType
00072 {
00073 RIGID_JOINT,
00074 PRISMATIC_JOINT,
00075 REVOLUTE_JOINT,
00076 DISCONNECTED_JOINT
00077 };
00078
00079 class JointFilter;
00080 typedef boost::shared_ptr<JointFilter> JointFilterPtr;
00081
00082 class JointFilter
00083 {
00084 public:
00085
00089 JointFilter();
00090
00094 virtual ~JointFilter();
00095
00102 JointFilterPtr clone() const
00103 {
00104 return (JointFilterPtr(doClone()));
00105 }
00106
00110 JointFilter(const JointFilter &joint);
00111
00112 virtual void initialize();
00113
00118 virtual void predictState(double time_interval_ns){}
00119
00127 virtual void predictMeasurement(){}
00128
00134 virtual void correctState(){}
00135
00141 virtual void setMeasurement(joint_measurement_t acquired_measurement, const double& measurement_timestamp_ns);
00142
00143 virtual void setInitialMeasurement(const joint_measurement_t &initial_measurement,
00144 const Eigen::Twistd& rrb_pose_at_srb_birth_in_sf,
00145 const Eigen::Twistd& srb_pose_at_srb_birth_in_sf);
00146
00152 virtual Eigen::Twistd getPredictedMeasurement() const;
00153
00159 virtual void estimateMeasurementHistoryLikelihood() {}
00160
00161 virtual void estimateUnnormalizedModelProbability() {}
00162
00167 virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame() = 0;
00168
00175 virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame() =0;
00176
00183 virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame() =0;
00184
00190 virtual void setNormalizingTerm(double normalizing_term);
00191
00198 virtual void setModelPriorProbability(double model_prior_probability);
00199
00206 virtual double getModelPriorProbability() const;
00207
00213 virtual double getLikelihoodOfLastMeasurements() const;
00214
00220 virtual void setLikelihoodOfLastMeasurements(double likelihood);
00221
00227 virtual double getUnnormalizedProbabilityOfJointFilter() const;
00228
00235 virtual double getProbabilityOfJointFilter() const;
00236
00242 virtual JointFilterType getJointFilterType() const = 0;
00243
00249 virtual std::string getJointFilterTypeStr() const = 0;
00250
00254 virtual std::vector<visualization_msgs::Marker> getJointMarkersInRRBFrame() const = 0;
00255
00256 virtual Eigen::Vector3d getJointPositionInRRBFrame() const;
00257
00258 virtual Eigen::Vector3d getJointOrientationRPYInRRBFrame() const;
00259
00260 virtual Eigen::Vector3d getJointOrientationUnitaryVector() const;
00261
00267 virtual JointCombinedFilterId getJointId() const;
00268
00269 virtual void setJointId(JointCombinedFilterId joint_id);
00270
00271 virtual double getOrientationPhiInRRBFrame() const;
00272
00273 virtual double getOrientationThetaInRRBFrame() const;
00274
00275 virtual double getCovarianceOrientationPhiPhiInRRBFrame() const;
00276
00277 virtual double getCovarianceOrientationThetaThetaInRRBFrame() const;
00278
00279 virtual double getCovarianceOrientationPhiThetaInRRBFrame() const;
00280
00281 virtual void setLoopPeriodNS(double loop_period_ns);
00282
00283 virtual double getLoopPeriodNS() const;
00284
00285 virtual int getNumSamplesForLikelihoodEstimation() const;
00286
00287 virtual void setNumSamplesForLikelihoodEstimation(int likelihood_sample_num);
00288
00289 virtual double getNormalizingTerm();
00290
00291 virtual void setCovariancePrior(double prior_cov_vel);
00292
00293 virtual void setCovarianceAdditiveSystemNoisePhi(double sigma_sys_noise_phi);
00294
00295 virtual void setCovarianceAdditiveSystemNoiseTheta(double sigma_sys_noise_theta);
00296
00297 virtual void setCovarianceAdditiveSystemNoisePx(double sigma_sys_noise_px);
00298
00299 virtual void setCovarianceAdditiveSystemNoisePy(double sigma_sys_noise_py);
00300
00301 virtual void setCovarianceAdditiveSystemNoisePz(double sigma_sys_noise_pz);
00302
00303 virtual void setCovarianceAdditiveSystemNoiseJointState(double sigma_sys_noise_pv);
00304
00305 virtual void setCovarianceAdditiveSystemNoiseJointVelocity(double sigma_sys_noise_pvd);
00306
00307 virtual void setCovarianceMeasurementNoise(double sigma_meas_noise);
00308
00314 virtual double getJointState() const;
00315
00321 virtual double getJointVelocity() const;
00322
00323 protected:
00324
00330 void _initVariables(JointCombinedFilterId joint_id);
00331
00332 JointCombinedFilterId _joint_id;
00333
00334
00335 int _likelihood_sample_num;
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346 double _measurements_likelihood;
00347 bool _externally_set_likelihood;
00348 double _model_prior_probability;
00349 double _normalizing_term;
00350
00351 double _unnormalized_model_probability;
00352
00353
00354 Eigen::Vector3d _joint_position;
00355
00356 Eigen::Matrix3d _uncertainty_joint_position;
00357
00358
00359 double _joint_orientation_phi;
00360 double _joint_orientation_theta;
00361
00362 Eigen::Vector3d _joint_orientation;
00363
00364 Eigen::Matrix2d _uncertainty_joint_orientation_phitheta;
00365
00366
00367 double _joint_state;
00368
00369 double _uncertainty_joint_state;
00370
00371
00372 std::vector<double> _joint_states_all;
00373
00374
00375 double _joint_velocity;
00376
00377 double _uncertainty_joint_velocity;
00378
00379
00380
00381 double _joint_acceleration;
00382
00383 double _uncertainty_joint_acceleration;
00384
00385 Eigen::Vector3d _rrb_centroid_in_sf;
00386 Eigen::Vector3d _srb_centroid_in_sf;
00387
00388
00389 double _prior_cov_vel;
00390 double _sigma_sys_noise_phi;
00391 double _sigma_sys_noise_theta;
00392 double _sigma_sys_noise_px;
00393 double _sigma_sys_noise_py;
00394 double _sigma_sys_noise_pz;
00395 double _sigma_sys_noise_jv;
00396 double _sigma_sys_noise_jvd;
00397 double _sigma_meas_noise;
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417 Eigen::Twistd _rrb_current_pose_in_sf;
00418 Eigen::Twistd _rrb_previous_pose_in_sf;
00419 Eigen::Matrix<double, 6, 6> _rrb_pose_cov_in_sf;
00420 Eigen::Twistd _rrb_current_vel_in_sf;
00421 Eigen::Matrix<double, 6, 6> _rrb_vel_cov_in_sf;
00422
00423 Eigen::Twistd _srb_current_pose_in_sf;
00424 Eigen::Matrix<double, 6, 6> _srb_pose_cov_in_sf;
00425 Eigen::Twistd _srb_current_vel_in_sf;
00426
00427
00428 Eigen::Twistd _srb_initial_pose_in_rrbf;
00429 Eigen::Matrix<double, 6, 6> _srb_initial_pose_cov_in_rrbf;
00430 Eigen::Twistd _srb_current_pose_in_rrbf;
00431 Eigen::Matrix<double, 6, 6> _srb_current_pose_cov_in_rrbf;
00432
00433 Eigen::Twistd _srb_previous_pose_in_rrbf;
00434 Eigen::Twistd _srb_predicted_pose_in_rrbf;
00435 Eigen::Twistd _srb_previous_predicted_pose_in_rrbf;
00436 Eigen::Twistd _current_delta_pose_in_rrbf;
00437 Eigen::Matrix<double, 6, 6> _current_delta_pose_cov_in_rrbf;
00438 Eigen::Twistd _previous_delta_pose_in_rrbf;
00439 Eigen::Twistd _predicted_delta_pose_in_rrbf;
00440 std::vector<Eigen::Twistd> _delta_poses_in_rrbf;
00441
00442 virtual JointFilter* doClone() const = 0;
00443
00444 double _loop_period_ns;
00445
00446 #ifdef PUBLISH_PREDICTED_POSE_AS_PWC
00447
00448 ros::NodeHandle _predicted_next_pose_nh;
00449 ros::Publisher _predicted_next_pose_publisher;
00450 #endif
00451
00452 tf::TransformBroadcaster _tf_pub;
00453
00454 double _measurement_timestamp_ns;
00455
00456 int _rrb_id;
00457
00458
00459 bool _inverted_delta_srb_pose_in_rrbf;
00460 bool _from_inverted_to_non_inverted;
00461 bool _from_non_inverted_to_inverted;
00462 };
00463
00464 }
00465
00466 #endif