Merge pull request #301 from robotology/fix/etapas

Added features for computing ground truth and improving results

app/gazebo/tug/plugins/tuginterface/interface/include/tuginterface.h

@@ -38,6 +38,8 @@ class TugInterface : public ModelPlugin
     physics::WorldPtr world;
     physics::ActorPtr actor;
     Velocity vel;
+    double walktime;
+    int nsteps;
     std::string starting_animation;
     yarp::sig::Matrix targets;
     std::map<double, ignition::math::Pose3d> waypoints_map;

app/gazebo/tug/plugins/tuginterface/interface/include/tugserver.h

@@ -15,6 +15,10 @@
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/World.hh>
 #include <gazebo/physics/Actor.hh>
+#include <gazebo/physics/Model.hh>
+#include <gazebo/physics/Link.hh>
+#include <gazebo/physics/Collision.hh>
+#include <gazebo/physics/BoxShape.hh>
 
 #include <include/utils.h>
 
@@ -28,6 +32,8 @@ class TugServer: public TugInterfaceServer
     gazebo::physics::WorldPtr world;
     gazebo::physics::ActorPtr actor;
     Velocity vel;
+    double walktime,standuptime,sitdowntime;
+    int nsteps;
     yarp::sig::Matrix targets;
     yarp::sig::Matrix line_frame;
 
@@ -54,6 +60,32 @@ class TugServer: public TugInterfaceServer
      */
     virtual double getSpeed();
 
+    /**
+     * Get time taken to complete the specified animation.
+     * @param animation_name name of the animation defined in the sdf.
+     * @param nsamples total number of samples for computing the average time.
+     * @return returns animation time.
+     */
+    virtual double getTime(const std::string &animation_name, const int nsamples);
+
+    /**
+     * Get walking time taken to complete the trajectory.
+     * @return returns walking time.
+     */
+    virtual double getWalkingTime();
+
+    /**
+     * Get time taken to stand up and sit down.
+     * @return returns stand up plus sit down time.
+     */
+    virtual double getStandSitTime();
+
+    /**
+     * Get number of steps.
+     * @return returns number of steps.
+     */
+    virtual int getNumSteps();
+
     /**
      * Get the list of animations associated with the actor.
      * @return returns list of animations associated with the actor.
@@ -97,6 +129,12 @@ class TugServer: public TugInterfaceServer
      */
     virtual bool playFromLast(const bool complete);
 
+    /**
+     * Get the torso front surface x coordinate.
+     * @return returns double defining the torso front surface x coordinate.
+     */
+    virtual double getTorsoFront();
+
     /**
      * Get current animation being played.
      * @return returns string defining the current animation being played.
@@ -134,7 +172,7 @@ class TugServer: public TugInterfaceServer
 
     void updateMap(const yarp::sig::Matrix &t);
 
-    void init(const Velocity &vel, const yarp::sig::Matrix &waypoints);
+    void init(const Velocity &vel, const yarp::sig::Matrix &waypoints, const double &walktime, const int &nsteps);
 
     void attachWorldPointer(gazebo::physics::WorldPtr p);
 

app/gazebo/tug/plugins/tuginterface/interface/include/utils.h

@@ -23,7 +23,7 @@ struct Velocity
 
 void updateScript(sdf::ElementPtr &actor_sdf, const std::map<double, ignition::math::Pose3d> &m);
 
-std::map<double, ignition::math::Pose3d> createMap(const yarp::sig::Matrix &t, const Velocity &vel);
+std::map<double, ignition::math::Pose3d> createMap(const yarp::sig::Matrix &t, const Velocity &vel, double &walktime, int &nsteps);
 
 std::map<double, ignition::math::Pose3d> generateWaypoints(const Velocity &vel,
                                                            const std::map<double, ignition::math::Pose3d> &m);

app/gazebo/tug/plugins/tuginterface/interface/src/tuginterface.cpp

@@ -120,7 +120,7 @@ void TugInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
         return;
     }
 
-    waypoints_map=createMap(targets,vel);
+    waypoints_map=createMap(targets,vel,walktime,nsteps);
     waypoints_map=generateWaypoints(vel,waypoints_map);
 
     sdf::ElementPtr world_sdf=world->SDF();
@@ -143,7 +143,7 @@ void TugInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
 
     m_rpcport.open(portname);
     server.yarp().attachAsServer(m_rpcport);
-    server.init(vel,targets);
+    server.init(vel,targets,walktime,nsteps);
 
     // Listen to the update event. This event is broadcast every
     // simulation iteration.

app/gazebo/tug/plugins/tuginterface/interface/src/tugserver.cpp

@@ -18,7 +18,8 @@ using namespace std;
 using namespace gazebo;
 
 /****************************************************************/
-TugServer::TugServer() : world(0), actor(0), vel(0.0,0.0)
+TugServer::TugServer() : world(0), actor(0), vel(0.0,0.0), walktime(0.0),
+    standuptime(0.0), sitdowntime(0.0), nsteps(0)
 {
     line_frame=yarp::math::eye(4,4);
 }
@@ -72,6 +73,22 @@ bool TugServer::playFromLast(const bool complete)
     return true;
 }
 
+/****************************************************************/
+double TugServer::getTorsoFront()
+{
+    auto link=actor->GetLink("LowerBack");
+    auto collision=link->GetCollisions()[0];
+    auto boxShape = boost::dynamic_pointer_cast<gazebo::physics::BoxShape>(
+            collision->GetShape());
+    auto size = boxShape->Size();
+
+    auto actor_pose=actor->WorldPose().Pos();
+    auto box_pose=collision->WorldPose().Pos();
+
+    double torso_front=box_pose.X()-actor_pose.X()+size[0]/2;
+    return torso_front;
+}
+
 /****************************************************************/
 string TugServer::getState()
 {
@@ -148,7 +165,9 @@ bool TugServer::setTarget(const Pose &p)
     ty[1]=t.Pos().Y();
     ty[2]=t.Rot().Yaw();
     targets.setRow(1,ty);
+    targets(2,0)=ty[0];
     updateMap(targets);
+    yDebug()<<"New target matrix"<<targets.toString();
     yInfo()<<"Setting target to"<<ty.toString();
     return true;
 }
@@ -168,6 +187,74 @@ double TugServer::getSpeed()
     return this->vel.lin_vel;
 }
 
+/****************************************************************/
+double TugServer::getTime(const string &animation_name, const int nsamples)
+{
+    double time=0.0;
+    for(int i=0; i<nsamples; i++)
+    {
+        double tstart=yarp::os::Time::now();
+        actor->PlayWithAnimationName(animation_name,false);
+        while(actor->IsActive())
+        {
+    //                yInfo()<<"Computing time for"<<animation;
+        }
+        double tend=yarp::os::Time::now()-tstart;
+        time+=tend;
+    }
+    time/=nsamples;
+    return time;
+}
+
+/****************************************************************/
+double TugServer::getStandSitTime()
+{
+    physics::Actor::SkeletonAnimation_M skel_m=actor->SkeletonAnimations();
+    double tottime=0.0;
+    for (auto it=skel_m.begin(); it!=skel_m.end(); it++)
+    {
+        string animation=it->first;
+        if (animation=="stand_up")
+        {
+            double tstart=yarp::os::Time::now();
+            actor->PlayWithAnimationName(animation,false);
+            while(actor->IsActive())
+            {
+//                yInfo()<<"Computing time for"<<animation;
+            }
+            this->standuptime=yarp::os::Time::now()-tstart;
+            yInfo()<<"Standup time"<<this->standuptime;
+        }
+        if (animation=="sit_down")
+        {
+            double tstart=yarp::os::Time::now();
+            actor->PlayWithAnimationName(animation,false);
+            while(actor->IsActive())
+            {
+//                yInfo()<<"Computing time for"<<animation;
+            }
+            this->sitdowntime=yarp::os::Time::now()-tstart;
+            yInfo()<<"Sitdown time"<<this->sitdowntime;
+        }
+        tottime=this->sitdowntime+this->standuptime;
+    }
+
+    return tottime;
+}
+
+
+/****************************************************************/
+double TugServer::getWalkingTime()
+{
+    return this->walktime;
+}
+
+/****************************************************************/
+int TugServer::getNumSteps()
+{
+    return this->nsteps;
+}
+
 /****************************************************************/
 std::vector<std::string> TugServer::getAnimationList()
 {
@@ -258,7 +345,7 @@ yarp::os::Property TugServer::getModelPos(const string &model_name)
 /****************************************************************/
 void TugServer::updateMap(const yarp::sig::Matrix &t)
 {
-    std::map<double, ignition::math::Pose3d> wp_map=createMap(t,this->vel);
+    std::map<double, ignition::math::Pose3d> wp_map=createMap(t,this->vel,this->walktime,this->nsteps);
     wp_map=generateWaypoints(this->vel,wp_map);
 
     sdf::ElementPtr world_sdf=world->SDF();
@@ -269,10 +356,14 @@ void TugServer::updateMap(const yarp::sig::Matrix &t)
 
 /****************************************************************/
 void TugServer::init(const Velocity &vel,
-                     const yarp::sig::Matrix &targets)
+                     const yarp::sig::Matrix &targets,
+                     const double &walktime,
+                     const int &nsteps)
 {
     this->vel=vel;
     this->targets=targets;
+    this->walktime=walktime;
+    this->nsteps=nsteps;
 }
 
 /****************************************************************/

app/gazebo/tug/plugins/tuginterface/interface/src/utils.cpp

@@ -44,15 +44,19 @@ void updateScript(sdf::ElementPtr &actor_sdf, const std::map<double, ignition::m
 }
 
 /****************************************************************/
-std::map<double, ignition::math::Pose3d> createMap(const yarp::sig::Matrix &t, const Velocity &vel)
+std::map<double, ignition::math::Pose3d> createMap(const yarp::sig::Matrix &t, const Velocity &vel, double &walktime, int &nsteps)
 {
     std::map<double, ignition::math::Pose3d> m;
     yarp::sig::Vector t0=t.getRow(0);
+    walktime=0.0;
     double duration=0.0;
+    double step_length=0.643;
+    nsteps=0;
     for (int i=0; i<t.rows(); i++)
     {
         yarp::sig::Vector t1=t.getRow(i);
         double dist=yarp::math::norm(t1.subVector(0,1)-t0.subVector(0,1));
+        nsteps+=ceil(dist/step_length);
         if (dist>0.0)
         {
             duration+=dist/vel.lin_vel;
@@ -67,6 +71,9 @@ std::map<double, ignition::math::Pose3d> createMap(const yarp::sig::Matrix &t, c
         m.insert(std::pair<double, ignition::math::Pose3d>(duration,p));
         t0=t1;
     }
+    yDebug()<<"updating map.. with duration"<<duration;
+    yDebug()<<"number of steps"<<nsteps;
+    walktime=duration;
 
     return m;
 }

app/gazebo/tug/plugins/tuginterface/thrift/tuginterface_rpc.thrift

@@ -43,6 +43,32 @@ service TugInterfaceServer
      */
     double getSpeed();
 
+    /**
+     * Get time taken to complete the specified animation.
+     * @param animation_name name of the animation defined in the sdf.
+     * @param nsamples total number of samples for computing the average time.
+     * @return returns animation time.
+     */
+    double getTime(1:string animation_name, 2:i32 nsamples);
+
+    /**
+     * Get walking time.
+     * @return returns walking time
+     */
+    double getWalkingTime();
+
+    /**
+     * Get stand up plus sit down time.
+     * @return returns stand up plus sit down time
+     */
+    double getStandSitTime();
+
+    /**
+     * Get number of steps.
+     * @return returns number of steps
+     */
+    i32 getNumSteps();
+
     /**
      * Get the list of animations associated with the actor.
      * @return returns list of animations associated with the actor.
@@ -85,6 +111,12 @@ service TugInterfaceServer
      */
     bool playFromLast(1: bool complete=false);
 
+    /**
+     * Get the torso front surface x coordinate.
+     * @return returns double defining the torso front surface x coordinate.
+     */
+    double getTorsoFront();
+
     /**
      * Get current animation being played.
      * @return returns string defining the current animation being played.