Commits

Mihai Emanuel Dolha committed c0250d0

fixed sync problem

  • Participants
  • Parent commits f82290c
  • Branches visual_sync

Comments (0)

Files changed (9)

File src/common/SkeletonAnimation.cc

   math::Matrix4 prevTrans = it2->second;
 
   double t = (time - prevKey) / (nextKey - prevKey);
-  if (this->name == "LeftToeBase")
-    std::cerr << "t: " << t << "\n";
   assert(t > 0.0 && t < 1.0);
 
   math::Vector3 nextPos = nextTrans.GetTranslation();

File src/math/Quaternion.cc

 //////////////////////////////////////////////////
 bool Quaternion::operator ==(const Quaternion &_qt) const
 {
-  return equal(this->x, _qt.x, 1e-10) &&
-         equal(this->y, _qt.y, 1e-10) &&
-         equal(this->z, _qt.z, 1e-10) &&
-         equal(this->w, _qt.w, 1e-10);
+  return equal(this->x, _qt.x, 1e-3) &&
+         equal(this->y, _qt.y, 1e-3) &&
+         equal(this->z, _qt.z, 1e-3) &&
+         equal(this->w, _qt.w, 1e-3);
 }
 
 //////////////////////////////////////////////////
 bool Quaternion::operator!=(const Quaternion &_qt) const
 {
-  return !equal(this->x, _qt.x, 1e-10) ||
-         !equal(this->y, _qt.y, 1e-10) ||
-         !equal(this->z, _qt.z, 1e-10) ||
-         !equal(this->w, _qt.w, 1e-10);
+  return !equal(this->x, _qt.x, 1e-3) ||
+         !equal(this->y, _qt.y, 1e-3) ||
+         !equal(this->z, _qt.z, 1e-3) ||
+         !equal(this->w, _qt.w, 1e-3);
 }
 
 //////////////////////////////////////////////////

File src/math/Vector3.cc

 //////////////////////////////////////////////////
 bool Vector3::operator ==(const Vector3 &_pt) const
 {
-  return equal(this->x, _pt.x, 1e-10) &&
-         equal(this->y, _pt.y, 1e-10) &&
-         equal(this->z, _pt.z, 1e-10);
+  return equal(this->x, _pt.x, 1e-3) &&
+         equal(this->y, _pt.y, 1e-3) &&
+         equal(this->z, _pt.z, 1e-3);
 }
 
 //////////////////////////////////////////////////

File src/physics/Actor.cc

     linkSdf->GetAttribute("gravity")->Set(false);
     sdf::ElementPtr linkPose = linkSdf->GetOrCreateElement("origin");
 
-    this->AddSphereInertia(linkSdf, math::Pose(), 1.0, 0.01);
+/*    this->AddSphereInertia(linkSdf, math::Pose(), 1.0, 0.01);
     this->AddSphereCollision(linkSdf, actorName + "_origin_col",
                                              math::Pose(), 0.02);
-    this->AddBoxVisual(linkSdf, actorName + "_origin_vis",
+*/    this->AddBoxVisual(linkSdf, actorName + "_origin_vis",
                     math::Pose(), math::Vector3(0.05, 0.05, 0.05), "Gazebo/White", Color::White);
     this->AddActorVisual(linkSdf, actorName + "_visual", math::Pose());
 
 
   frame[skelMap[this->skeleton->GetRootNode()->GetName()]] = rootM;
 
-  printf("script time: %f\n", scriptTime);
   this->SetPose(frame, skelMap, currentTime.Double());
 
   this->lastScriptTime = scriptTime;
-  printf("======================================\n");
-//  physics::pause_worlds(true);
 }
 
 //////////////////////////////////////////////////
       bonePose.Correct();
     }
 
-    msgs::Pose *msg_pose = msg.add_pose();
-    msg_pose->set_name(bone->GetName());
+    msgs::Pose *bone_pose = msg.add_pose();
+    bone_pose->set_name(bone->GetName());
+
     if (!parentBone)
     {
-      msg_pose->mutable_position()->CopyFrom(msgs::Convert(math::Vector3()));
-      msg_pose->mutable_orientation()->CopyFrom(msgs::Convert(
-                                                        math::Quaternion()));
-      currentLink->SetWorldPose(bonePose);
+      bone_pose->mutable_position()->CopyFrom(msgs::Convert(math::Vector3()));
+      bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(
+                                                    math::Quaternion()));
       mainLinkPose = bonePose;
     }
     else
     {
-      msg_pose->mutable_position()->CopyFrom(msgs::Convert(bonePose.pos));
-      msg_pose->mutable_orientation()->CopyFrom(msgs::Convert(bonePose.rot));
+      bone_pose->mutable_position()->CopyFrom(msgs::Convert(bonePose.pos));
+      bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(bonePose.rot));
       LinkPtr parentLink = this->GetChildLink(parentBone->GetName());
       math::Pose parentPose = parentLink->GetWorldPose();
       math::Matrix4 parentTrans(parentPose.rot.GetAsMatrix4());
       parentTrans.SetTranslate(parentPose.pos);
       transform = parentTrans * transform;
-      currentLink->SetWorldPose(transform.GetAsPose());
-      if (bone->GetName() == "LeftToeBase")
-      {
-        math::Vector3 toepos = transform.GetTranslation();
-        printf("toe pos: %f %f %f\n", toepos.x, toepos.y, toepos.z);
-        msgs::Pose *msg_pos1 = msg.add_pose();
-        math::Pose toePose = transform.GetAsPose();
-        msg_pos1->set_name("toe");
-        msg_pos1->mutable_position()->CopyFrom(msgs::Convert(toePose.pos));
-        msg_pos1->mutable_orientation()->CopyFrom(msgs::Convert(toePose.rot));
-      }
     }
+
+    msgs::Pose *link_pose = msg.add_pose();
+    link_pose->set_name(currentLink->GetScopedName());
+    math::Pose linkPose = transform.GetAsPose() - mainLinkPose;
+    link_pose->mutable_position()->CopyFrom(msgs::Convert(linkPose.pos));
+    link_pose->mutable_orientation()->CopyFrom(msgs::Convert(linkPose.rot));
+
+    currentLink->SetWorldPose(transform.GetAsPose(), true, false);
   }
 
   msgs::Time *stamp = msg.add_time();
   stamp->CopyFrom(msgs::Convert(_time));
 
+  msgs::Pose *model_pose = msg.add_pose();
+  model_pose->set_name(this->GetScopedName());
+  model_pose->mutable_position()->CopyFrom(msgs::Convert(mainLinkPose.pos));
+  model_pose->mutable_orientation()->CopyFrom(msgs::Convert(mainLinkPose.rot));
+
   if (this->bonePosePub && this->bonePosePub->HasConnections())
     this->bonePosePub->Publish(msg);
-  this->mainLink->SetWorldPose(mainLinkPose);
-  printf("root pos: %f %f %f\n", mainLinkPose.pos.x, mainLinkPose.pos.y, mainLinkPose.pos.z);
+  this->SetWorldPose(mainLinkPose, true, false);
 }
 
 //////////////////////////////////////////////////

File src/physics/Entity.cc

 }
 
 //////////////////////////////////////////////////
-void Entity::SetRelativePose(const math::Pose &_pose, bool _notify)
+void Entity::SetRelativePose(const math::Pose &_pose, bool _notify,
+        bool _publish)
 {
   if (this->parent && this->parentEntity)
-    this->SetWorldPose(_pose + this->parentEntity->GetWorldPose(), _notify);
+    this->SetWorldPose(_pose + this->parentEntity->GetWorldPose(), _notify,
+                              _publish);
   else
-    this->SetWorldPose(_pose, _notify);
+    this->SetWorldPose(_pose, _notify, _publish);
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void Entity::SetWorldPoseModel(const math::Pose &_pose, bool _notify)
+void Entity::SetWorldPoseModel(const math::Pose &_pose, bool _notify,
+        bool _publish)
 {
   math::Pose oldModelWorldPose = this->worldPose;
 
       else
       {
         entity->worldPose = ((entity->worldPose - oldModelWorldPose) + _pose);
-        entity->PublishPose();
+        if (_publish)
+          entity->PublishPose();
       }
 
       if (_notify)
 }
 
 //////////////////////////////////////////////////
-void Entity::SetWorldPoseCanonicalLink(const math::Pose &_pose, bool _notify)
+void Entity::SetWorldPoseCanonicalLink(const math::Pose &_pose, bool _notify,
+        bool _publish)
 {
   this->worldPose = _pose;
   this->worldPose.Correct();
     if (_notify)
       this->parentEntity->UpdatePhysicsPose(false);
 
-    this->parentEntity->PublishPose();
+    if (_publish)
+      this->parentEntity->PublishPose();
   }
   else
     gzerr << "SWP for CB[" << this->GetName() << "] but parent["
 }
 
 //////////////////////////////////////////////////
-void Entity::SetWorldPoseDefault(const math::Pose &_pose, bool _notify)
+void Entity::SetWorldPoseDefault(const math::Pose &_pose, bool _notify,
+        bool /*_publish*/)
 {
   this->worldPose = _pose;
   this->worldPose.Correct();
 //    MWP  - Model World Pose
 //    CBRP - Canonical Body Relative (to Model) Pose
 //
-void Entity::SetWorldPose(const math::Pose &_pose, bool _notify)
+void Entity::SetWorldPose(const math::Pose &_pose, bool _notify, bool _publish)
 {
   this->GetWorld()->setWorldPoseMutex->lock();
 
-  (*this.*setWorldPoseFunc)(_pose, _notify);
+  (*this.*setWorldPoseFunc)(_pose, _notify, _publish);
 
   this->GetWorld()->setWorldPoseMutex->unlock();
 
-  this->PublishPose();
+  if (_publish)
+    this->PublishPose();
 }
 
 //////////////////////////////////////////////////

File src/physics/Entity.hh

       /// \brief Set the pose of the entity relative to its parent
       /// \param pose The new pose
       /// \param notify True = tell children of the pose change
-      public: void SetRelativePose(const math::Pose &pose, bool notify = true);
+      public: void SetRelativePose(const math::Pose &pose, bool notify = true,
+                                   bool publish = true);
 
       /// \brief Set the world pose of the entity
       /// \param pose The new world pose
       /// \param notify True = tell children of the pose change
-      public: void SetWorldPose(const math::Pose &pose, bool notify = true);
+      public: void SetWorldPose(const math::Pose &pose, bool notify = true,
+                                bool publish = true);
 
       /// \brief Get the linear velocity of the entity
       /// \return A math::Vector3 for the linear velocity
 
       private: math::Box GetCollisionBoundingBoxHelper(BasePtr _base) const;
 
-      private: void SetWorldPoseModel(const math::Pose &_pose, bool _notify);
+      private: void SetWorldPoseModel(const math::Pose &_pose, bool _notify,
+                                      bool _publish);
 
       private: void SetWorldPoseCanonicalLink(const math::Pose &_pose,
-                                              bool _notify);
+                                              bool _notify, bool _publish);
 
-      private: void SetWorldPoseDefault(const math::Pose &_pose, bool _notify);
+      private: void SetWorldPoseDefault(const math::Pose &_pose, bool _notify,
+                                        bool _publish);
 
       /// \brief Called when a new pose message arrives
       private: void OnPoseMsg(ConstPosePtr &_msg);
       protected: math::Pose dirtyPose;
       private: boost::function<void()> onAnimationComplete;
 
-      private: void (Entity::*setWorldPoseFunc)(const math::Pose &, bool);
+      private: void (Entity::*setWorldPoseFunc)(const math::Pose &, bool, bool);
     };
 
     /// \}

File src/rendering/Scene.cc

   }
   this->lightMsgs.clear();
 
-
   // Process all the model messages last. Remove pose message from the list
   // only when a corresponding visual exits. We may receive pose updates
   // over the wire before  we recieve the visual
           std::string::npos)
       {
         math::Pose pose = msgs::Convert(*(*pIter));
-        if ((*pIter)->name() == "actor1")
-           printf("processing pose for actor1: %f %f %f\n", pose.pos.x, pose.pos.y, pose.pos.z);
         iter->second->SetPose(pose);
       }
       PoseMsgs_L::iterator prev = pIter++;
   while (spIter != this->skeletonPoseMsgs.end())
   {
     Visual_M::iterator iter = this->visuals.find((*spIter)->model_name());
+    for (int i = 0; i < (*spIter)->pose_size(); i++)
+    {
+      const msgs::Pose& pose_msg = (*spIter)->pose(i);
+      Visual_M::iterator iter2 = this->visuals.find(pose_msg.name());
+      if (iter2 != this->visuals.end())
+      {
+        // If an object is selected, don't let the physics engine move it.
+        if (this->selectionObj->GetVisualName().empty() ||
+           !this->selectionObj->IsActive() ||
+           iter->first.find(this->selectionObj->GetVisualName()) ==
+            std::string::npos)
+        {
+          math::Pose pose = msgs::Convert(pose_msg);
+          iter2->second->SetPose(pose);
+        }
+      }
+    }
+
     if (iter != this->visuals.end())
     {
-      printf("processing skeleton pose\n");
       iter->second->SetSkeletonPose(*(*spIter).get());
       SkeletonPoseMsgs_L::iterator prev = spIter++;
       this->skeletonPoseMsgs.erase(prev);
   boost::mutex::scoped_lock lock(*this->receiveMutex);
   PoseMsgs_L::iterator iter;
 
-  if (_msg->name() == "actor1" || _msg->name() == "actor1::LeftToeBase")
-  {
-    printf("received %s pose: %f %f %f\n", _msg->name().c_str(),
-                                           _msg->position().x(),
-                                           _msg->position().y(),
-                                           _msg->position().z());
-  }
-
   // Find an old model message, and remove them
   for (iter = this->poseMsgs.begin(); iter != this->poseMsgs.end(); ++iter)
   {
 {
   boost::mutex::scoped_lock lock(*this->receiveMutex);
   SkeletonPoseMsgs_L::iterator iter;
-  printf("received skeleton pose\n");
 
   // Find an old model message, and remove them
   for (iter = this->skeletonPoseMsgs.begin();

File src/rendering/Scene.hh

       private: transport::SubscriberPtr responseSub;
       private: transport::SubscriberPtr requestSub;
       private: transport::SubscriberPtr skeletonPoseSub;
+      private: transport::SubscriberPtr triggerSub;
       private: transport::PublisherPtr requestPub;
 
       private: std::vector<event::ConnectionPtr> connections;

File src/rendering/Visual.cc

     gzerr << "Visual " << this->GetName() << " has no skeleton.\n";
     return;
   }
-  this->sceneNode->getParent()->_update(true, true);
-
-  Ogre::Bone *foot = NULL;
-  Ogre::Vector3 toePos;
 
   for (int i = 0; i < _pose.pose_size(); i++)
   {
     const msgs::Pose& bonePose = _pose.pose(i);
     if (!this->skeleton->hasBone(bonePose.name()))
-    {
-      gzerr << "Bone " << bonePose.name() << " not found.\n";
-      toePos.x = bonePose.position().x();
-      toePos.y = bonePose.position().y();
-      toePos.z = bonePose.position().z();
       continue;
-    }
     Ogre::Bone *bone = this->skeleton->getBone(bonePose.name());
     Ogre::Vector3 p(bonePose.position().x(),
                     bonePose.position().y(),
     bone->setManuallyControlled(true);
     bone->setPosition(p);
     bone->setOrientation(quat);
-    if (bone->getName() == "LeftToeBase")
-    {
-      foot = bone;
-    }
   }
-
-  Ogre::Entity *ent = NULL;
-
-  for (int i = 0; i < this->sceneNode->numAttachedObjects(); ++i)
-  {
-    ent = dynamic_cast<Ogre::Entity*>(this->sceneNode->getAttachedObject(i));
-    if (ent->hasSkeleton())
-      break;
-  }
-//  this->scene->GetManager()->_applySceneAnimations();
-  Ogre::Vector3 rootpos = this->sceneNode->_getFullTransform().getTrans();
-  printf("root pos: %f %f %f\n", rootpos.x, rootpos.y, rootpos.z);
-  Ogre::Vector3 ogretoePos = (this->sceneNode->_getFullTransform() *
-                                  foot->_getFullTransform()).getTrans();
-  printf("skeleton toe pos: %f %f %f\n", ogretoePos.x, ogretoePos.y, ogretoePos.z);
-  printf("physical toe pos: %f %f %f\n", toePos.x, toePos.y, toePos.z);
-  Ogre::Vector3 error = toePos - ogretoePos;
-  printf("error: %f\n", error.length());
-  if (error.length() > 1e-3)
-  {
-    printf("\n\nERROR\n\n");
-    gzthrow("failed to sync");
-  }
-  printf("----------------------------------------------------\n");
 }