diff --git a/hubero_gazebo/include/hubero_gazebo/localisation_gazebo.h b/hubero_gazebo/include/hubero_gazebo/localisation_gazebo.h index d9fa7b26..7b1591ac 100644 --- a/hubero_gazebo/include/hubero_gazebo/localisation_gazebo.h +++ b/hubero_gazebo/include/hubero_gazebo/localisation_gazebo.h @@ -1,6 +1,7 @@ #pragma once #include +#include namespace hubero { @@ -12,19 +13,61 @@ namespace hubero { */ class LocalisationGazebo: public LocalisationBase { public: + /// Default constructor LocalisationGazebo(); - virtual void updateSimulator(const Pose3& pose) override; + /** + * @brief Extended version of updateSimulator call that realizes workaround for Gazebo-specific issues + * + * Provides timestamp to compute velocity and acceleration. + * + * Since Gazebo does not store velocity or acceleration of the ActorPlugin, an extended version + * of updateSimulator must be used to provide valid velocity and acceleration + */ + void updateSimulator(const Pose3& pose, const Time& time); - virtual void updateSimulator( + /** + * @brief Extended version of updateSimulator call that realizes workaround for Gazebo-specific issues + * + * For details see other @ref updateSimulator + */ + void updateSimulator( const Pose3& pose, const Vector3& vel_lin, const Vector3& vel_ang, const Vector3& acc_lin, - const Vector3& acc_ang - ) override; + const Vector3& acc_ang, + const Time& time + ); + /** + * @brief Retrieves the newest pose complement with simulator coordinate system + * + * The newest pose must be set by either @ref updateSimulator or @ref update + */ virtual Pose3 getPoseSimulator() const override; + +protected: + /** + * @brief Computes linear and angular velocities and accelerations based on given pose and stored vel and acc + * + * This must be called before any change to member, e.g., @ref pose_, @ref vel_ang_ or @ref vel_lin_ occurs + * + * @param pose current pose expressed in the target frame (HuBeRo's) + * @param time current timestamp + */ + void computeVelocityAndAcceleration(Pose3 pose, Time time); + + /** + * @defgroup cvamembers Localisation state at the time of last velocity/acceleration computation + * @{ + */ + Time time_cva_last_; + Pose3 pose_cva_last_; + Vector3 vel_lin_cva_last_; + Vector3 vel_ang_cva_last_; + + /// @} }; } // namespace hubero diff --git a/hubero_gazebo/src/actor_plugin_gazebo.cpp b/hubero_gazebo/src/actor_plugin_gazebo.cpp index 1dbd28ab..4a58cd02 100644 --- a/hubero_gazebo/src/actor_plugin_gazebo.cpp +++ b/hubero_gazebo/src/actor_plugin_gazebo.cpp @@ -49,7 +49,7 @@ void ActorPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) { * Update pose. Note that coordinate system of the human model is different to ROS REP 105 * https://www.ros.org/reps/rep-0105.html */ - sim_localisation_ptr_->updateSimulator(actor_ptr_->WorldPose()); + sim_localisation_ptr_->updateSimulator(actor_ptr_->WorldPose(), hubero::Time()); actor_ptr_->SetWorldPose(sim_localisation_ptr_->getPoseSimulator()); /* @@ -107,8 +107,8 @@ void ActorPlugin::OnUpdate(const common::UpdateInfo& info) { /* * Handle simulation update */ - sim_localisation_ptr_->updateSimulator(actor_ptr_->WorldPose()); hubero::Time time(info.simTime.Double()); + sim_localisation_ptr_->updateSimulator(actor_ptr_->WorldPose(), time); hubero_actor_.update(time); // TODO: parameterize animation factor, e.g. take from SDF diff --git a/hubero_gazebo/src/localisation_gazebo.cpp b/hubero_gazebo/src/localisation_gazebo.cpp index 9c4caf16..81b8aa2d 100644 --- a/hubero_gazebo/src/localisation_gazebo.cpp +++ b/hubero_gazebo/src/localisation_gazebo.cpp @@ -5,7 +5,7 @@ namespace hubero { LocalisationGazebo::LocalisationGazebo(): LocalisationBase::LocalisationBase() {} -void LocalisationGazebo::updateSimulator(const Pose3& pose) { +void LocalisationGazebo::updateSimulator(const Pose3& pose, const Time& time) { if (!isInitialized()) { HUBERO_LOG("[LocalisationGazebo] 'update' call could not be processed due to lack of initialization\r\n"); return; @@ -28,19 +28,21 @@ void LocalisationGazebo::updateSimulator(const Pose3& pose) { yaw.Normalize(); rpy.Z() = yaw.Radian(); - LocalisationBase::update(Pose3(pos, Quaternion(rpy))); + auto pose_new = Pose3(pos, Quaternion(rpy)); + computeVelocityAndAcceleration(pose_new, time); + LocalisationBase::update(pose_new); } void LocalisationGazebo::updateSimulator( const Pose3& pose, - const Vector3& vel_lin, - const Vector3& vel_ang, - const Vector3& acc_lin, - const Vector3& acc_ang + const Vector3& /*vel_lin*/, + const Vector3& /*vel_ang*/, + const Vector3& /*acc_lin*/, + const Vector3& /*acc_ang*/, + const Time& time ) { - LocalisationBase::update(pose, vel_lin, vel_ang, acc_lin, acc_ang); - // overwrite pose according to customized calculations (LocalisationGazebo::updateSimulator) - updateSimulator(pose); + // will overwrite pose, velocities and accelerations + updateSimulator(pose, time); } Pose3 LocalisationGazebo::getPoseSimulator() const { @@ -58,4 +60,33 @@ Pose3 LocalisationGazebo::getPoseSimulator() const { return Pose3(getPose().Pos(), Quaternion(rpy_gazebo)); } +void LocalisationGazebo::computeVelocityAndAcceleration(Pose3 pose, Time time) { + // time difference to compute velocity + double time_diff = Time::computeDuration(time_cva_last_, time).getTime(); + + // compute velocity (pose_ is previous pose (class member)) + vel_lin_.X((pose.Pos().X() - pose_cva_last_.Pos().X()) / time_diff); + vel_lin_.Y((pose.Pos().Y() - pose_cva_last_.Pos().Y()) / time_diff); + vel_lin_.Z((pose.Pos().Z() - pose_cva_last_.Pos().Z()) / time_diff); + + vel_ang_.X((pose.Rot().Roll() - pose_cva_last_.Rot().Roll()) / time_diff); + vel_ang_.Y((pose.Rot().Pitch() - pose_cva_last_.Rot().Pitch()) / time_diff); + vel_ang_.Z((pose.Rot().Yaw() - pose_cva_last_.Rot().Yaw()) / time_diff); + + // compute acceleration + acc_lin_.X((vel_lin_.X() - vel_lin_cva_last_.X()) / time_diff); + acc_lin_.Y((vel_lin_.Y() - vel_lin_cva_last_.Y()) / time_diff); + acc_lin_.Z((vel_lin_.Z() - vel_lin_cva_last_.Z()) / time_diff); + + acc_ang_.X((vel_ang_.X() - vel_ang_cva_last_.X()) / time_diff); + acc_ang_.Y((vel_ang_.Y() - vel_ang_cva_last_.Y()) / time_diff); + acc_ang_.Z((vel_ang_.Z() - vel_ang_cva_last_.Z()) / time_diff); + + // store for the next computations + time_cva_last_ = time; + pose_cva_last_ = pose; + vel_lin_cva_last_ = vel_lin_; + vel_ang_cva_last_ = vel_ang_; +} + } // namespace hubero