hubero_gazebo - Localisation: added manual velocity and acceleration …

…computations as ActorPlugin issue workaround

hubero_gazebo/include/hubero_gazebo/localisation_gazebo.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <hubero_interfaces/localisation_base.h>
+#include <hubero_common/time.h>
 
 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

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

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