Merge pull request #152 from ipab-slmc/wxm-fix-setmodelstate

KinematicTree: Fix setModelState (broken in the last ~3 weeks?)
ipab-slmc · Oct 17, 2017 · 262ba43 · 262ba43
2 parents 11dddcb + c6d1115
commit 262ba43
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/exotica/include/exotica/KinematicTree.h b/exotica/include/exotica/KinematicTree.h
@@ -200,7 +200,7 @@ class KinematicTree : public Uncopyable
     void BuildTree(const KDL::Tree& RobotKinematics);
     void AddElement(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent);
     int IsControlled(std::shared_ptr<KinematicElement> Joint);
-    void UpdateTree(Eigen::VectorXdRefConst x);
+    void UpdateTree();
     void UpdateFK();
     void UpdateJ();
     void ComputeJ(const KinematicFrame& frame, KDL::Jacobian& J);

diff --git a/exotica/src/KinematicTree.cpp b/exotica/src/KinematicTree.cpp
@@ -447,18 +447,18 @@ std::shared_ptr<KinematicResponse> KinematicTree::RequestFrames(const Kinematics
 void KinematicTree::Update(Eigen::VectorXdRefConst x)
 {
     if (x.rows() != StateSize) throw_pretty("Wrong state vector size! Got " << x.rows() << " expected " << StateSize);
-    UpdateTree(x);
+
+    for (int i = 0; i < ControlledJoints.size(); i++)
+        TreeState(ControlledJoints[i]->Id) = x(i);
+
+    UpdateTree();
     UpdateFK();
     if (Flags & KIN_J) UpdateJ();
     if (Debug) publishFrames();
 }
 
-void KinematicTree::UpdateTree(Eigen::VectorXdRefConst x)
+void KinematicTree::UpdateTree()
 {
-    for (int i = 0; i < ControlledJoints.size(); i++)
-    {
-        TreeState(ControlledJoints[i]->Id) = x(i);
-    }
     for (std::shared_ptr<KinematicElement> element : Tree)
     {
         KDL::Frame ParentFrame;
@@ -744,7 +744,7 @@ void KinematicTree::setModelState(Eigen::VectorXdRefConst x)
     {
         TreeState(ModelJointsMap.at(ModelJointsNames[i])->Id) = x(i);
     }
-
+    UpdateTree();
     UpdateFK();
     if (Flags & KIN_J) UpdateJ();
     if (Debug) publishFrames();
@@ -763,7 +763,7 @@ void KinematicTree::setModelState(std::map<std::string, double> x)
             throw_pretty("Robot model does not contain joint '" << joint.first << "'");
         }
     }
-
+    UpdateTree();
     UpdateFK();
     if (Flags & KIN_J) UpdateJ();
     if (Debug) publishFrames();