[frames] renaming, bindings, RobotWrapper, deprecating

bindings/python/algorithm/expose-frames.cpp

@@ -45,7 +45,7 @@ namespace se3
                                                )
     {
       computeJointJacobians(model,data,q);
-      framesForwardKinematics(model,data);
+      updateFramePlacements(model,data);
   
       return get_frame_jacobian_proxy(model, data, frame_id, rf);
     }
@@ -74,21 +74,59 @@ namespace se3
                                                               )
     {
       computeJointJacobiansTimeVariation(model,data,q,v);
-      framesForwardKinematics(model,data);
+      updateFramePlacements(model,data);
   
       return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
     }        
 
+    static Motion get_frame_velocity_proxy(const Model & model,
+                                           Data & data,
+                                           const Model::FrameIndex frame_id
+                                           )
+    {
+      Motion v;
+      getFrameVelocity(model,data,frame_id,v);
+      return v;
+    }
+
+    static Motion get_frame_acceleration_proxy(const Model & model,
+                                               Data & data,
+                                               const Model::FrameIndex frame_id
+                                               )
+    {
+      Motion a;
+      getFrameAcceleration(model,data,frame_id,a);
+      return a;
+    }
     
     void exposeFramesAlgo()
     {
-      bp::def("framesKinematics",
-              (void (*)(const Model &, Data &))&framesForwardKinematics,
+      bp::def("updateFramePlacements",
+              (void (*)(const Model &, Data &))&updateFramePlacements,
               bp::args("Model","Data"),
               "Computes the placements of all the operational frames according to the current joint placement stored in data"
-              "and put the results in data.");
+              "and puts the results in data.");
+
+      bp::def("updateFramePlacement",
+              (const SE3 & (*)(const Model &, Data &, const Model::FrameIndex))&updateFramePlacement,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Computes the placement of the given operational frames according to the current joint placement stored in data,"
+              "puts the results in data and returns it.",
+              bp::return_value_policy<bp::return_by_value>());
+
+      bp::def("getFrameVelocity",
+              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
+              "Fist or second order forwardKinematics should be called first.");
+
+      bp::def("getFrameAcceleration",
+              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
+              "Second order forwardKinematics should be called first.");
       
-      bp::def("framesKinematics",
+      bp::def("framesForwardKinematics",
               (void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
               bp::args("Model","Data",
                        "Configuration q (size Model::nq)"),

bindings/python/scripts/deprecated.py

@@ -21,6 +21,13 @@ from __future__ import print_function
 from . import libpinocchio_pywrap as se3 
 from .deprecation import deprecated
 
+@deprecated("This function has been renamed updateFramePlacements when taking two arguments, and framesForwardKinematics when taking three. Please change to the appropriate method.")
+def framesKinematics(model,data,q=None):
+  if q is None:
+    se3.updateFramePlacements(model,data)
+  else:
+    se3.framesForwardKinematics(model,data,q)
+
 @deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobians.")
 def computeJacobians(model,data,q=None):
   if q is None:

bindings/python/scripts/robot_wrapper.py

@@ -99,7 +99,11 @@ class RobotWrapper(object):
         else:
             se3.forwardKinematics(self.model, self.data, q)
 
+    @deprecated("This method is now renamed placement. Please use placement instead.")
     def position(self, q, index, update_kinematics=True):
+        return self.placement(q, index, update_kinematics)
+
+    def placement(self, q, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q)
         return self.data.oMi[index]
@@ -114,32 +118,29 @@ class RobotWrapper(object):
             se3.forwardKinematics(self.model, self.data, q, v, a)
         return self.data.a[index]
 
+    @deprecated("This method is now renamed framePlacement. Please use framePlacement instead.")
     def framePosition(self, q, index, update_kinematics=True):
+        return self.framePlacement(q, index, update_kinematics)
+
+    def framePlacement(self, q, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q)
-        frame = self.model.frames[index]
-        parentPos = self.data.oMi[frame.parent]
-        return parentPos.act(frame.placement)
+        return se3.updateFramePlacement(self.model, self.data, index)
 
     def frameVelocity(self, q, v, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q, v)
-        frame = self.model.frames[index]
-        parentJointVel = self.data.v[frame.parent]
-        return frame.placement.actInv(parentJointVel)
+        return se3.getFrameVelocity(self.model, self.data, index)
 
     def frameAcceleration(self, q, v, a, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q, v, a)
-        frame = self.model.frames[index]
-        parentJointAcc = self.data.a[frame.parent]
-        return frame.placement.actInv(parentJointAcc)
+        return se3.getFrameAcceleration(self.model, self.data, index)
 
     def frameClassicAcceleration(self, index):
-        f = self.model.frames[index]
-        a = f.placement.actInv(self.data.a[f.parent])
-        v = f.placement.actInv(self.data.v[f.parent])
-        a.linear += np.cross(v.angular.T, v.linear.T).T
+        v = se3.getFrameVelocity(self.model, self.data, index)
+        a = se3.getFrameAcceleration(self.model, self.data, index)
+        a.linear += np.cross(v.angular, v.linear, axis=0)
         return a;
 
     @deprecated("This method is now deprecated. Please use jointJacobian instead. It will be removed in release 1.4.0 of Pinocchio.")
@@ -175,9 +176,12 @@ class RobotWrapper(object):
         else:
             se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
 
-
+    @deprecated("This method is now renamed framesForwardKinematics. Please use framesForwardKinematics instead.")
     def framesKinematics(self, q): 
-        se3.framesKinematics(self.model, self.data, q)
+        se3.framesForwardKinematics(self.model, self.data, q)
+
+    def framesForwardKinematics(self, q): 
+        se3.framesForwardKinematics(self.model, self.data, q)
     
     '''
         It computes the Jacobian of frame given by its id (frame_id) either expressed in the
@@ -188,7 +192,7 @@ class RobotWrapper(object):
 
     '''
         Similar to getFrameJacobian but it also calls before se3.computeJointJacobians and
-        se3.framesKinematics to update internal value of self.data related to frames.
+        se3.framesForwardKinematics to update internal value of self.data related to frames.
     '''
     def frameJacobian(self, q, frame_id, rf_frame):
         return se3.frameJacobian(self.model, self.data, q, frame_id, rf_frame)

src/algorithm/frames.hpp

@@ -32,6 +32,35 @@ namespace se3
    *
    * @warning    One of the algorithms forwardKinematics should have been called first
    */
+  inline void updateFramePlacements(const Model & model,
+                                    Data & data);
+
+  /**
+   * @brief      Updates the placement of the given frame.
+   *
+   * @param[in]  model        The kinematic model.
+   * @param      data         Data associated to model.
+   * @param[in]  frame_id     Id of the operational Frame.
+   *
+   * @return     A reference to the frame placement stored in data.oMf[frame_id]
+   *
+   * @warning    One of the algorithms forwardKinematics should have been called first
+   */
+  inline const SE3 & updateFramePlacement(const Model & model,
+                                          Data & data,
+                                          const Model::FrameIndex frame_id);
+
+  /**
+   * @brief      Updates the placement of each frame contained in the model
+   *
+   * @deprecated This function is now deprecated. Please call se3::updateFramePlacements for same functionality
+   *
+   * @param[in]  model  The kinematic model.
+   * @param      data   Data associated to model.
+   *
+   * @warning    One of the algorithms forwardKinematics should have been called first
+   */
+  PINOCCHIO_DEPRECATED
   inline void framesForwardKinematics(const Model & model,
                                       Data & data);
 
@@ -47,21 +76,6 @@ namespace se3
                                       Data & data,
                                       const Eigen::VectorXd & q);
 
-  /**
-   * @brief      Updates the placement of the given frame.
-   *
-   * @param[in]  model        The kinematic model.
-   * @param      data         Data associated to model.
-   * @param[in]  frame_id     Id of the operational Frame.
-   *
-   * @return     A reference to the frame placement stored in data.oMf[frame_id]
-   *
-   * @warning    One of the algorithms forwardKinematics should have been called first
-   */
-  inline const SE3 & frameForwardKinematics(const Model & model,
-                                            Data & data,
-                                            const Model::FrameIndex frame_id);
-
   /**
    * @brief      Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
    *             You must first call se3::forwardKinematics to update placement and velocity values in data structure.

src/algorithm/frames.hxx

@@ -26,13 +26,13 @@ namespace se3
 {
   
   
-  inline void framesForwardKinematics(const Model & model,
-                                      Data & data)
+  inline void updateFramePlacements(const Model & model,
+                                    Data & data)
   {
     assert(model.check(data) && "data is not consistent with model.");
     
     // The following for loop starts by index 1 because the first frame is fixed
-    // and corresponds to the universe.s
+    // and corresponds to the universe
     for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
     {
       const Frame & frame = model.frames[i];
@@ -43,20 +43,10 @@ namespace se3
         data.oMf[i] = data.oMi[parent]*frame.placement;
     }
   }
-  
-  inline void framesForwardKinematics(const Model & model,
-                                      Data & data,
-                                      const Eigen::VectorXd & q)
-  {
-    assert(model.check(data) && "data is not consistent with model.");
-    
-    forwardKinematics(model, data, q);
-    framesForwardKinematics(model, data);
-  }
 
-  inline const SE3 & frameForwardKinematics(const Model & model,
-                                            Data & data,
-                                            const Model::FrameIndex frame_id)
+  inline const SE3 & updateFramePlacement(const Model & model,
+                                          Data & data,
+                                          const Model::FrameIndex frame_id)
   {
     const Frame & frame = model.frames[frame_id];
     const Model::JointIndex & parent = frame.parent;
@@ -67,6 +57,22 @@ namespace se3
     return data.oMf[frame_id];
   }
 
+  inline void framesForwardKinematics(const Model & model,
+                                      Data & data)
+  {
+    updateFramePlacements(model,data);
+  }
+
+  inline void framesForwardKinematics(const Model & model,
+                                      Data & data,
+                                      const Eigen::VectorXd & q)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    forwardKinematics(model, data, q);
+    updateFramePlacements(model, data);
+  }
+
   void getFrameVelocity(const Model & model,
                         const Data & data,
                         const Model::FrameIndex frame_id,

unittest/frames.cpp

@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
 
 }
 
-BOOST_AUTO_TEST_CASE ( test_single_kinematics )
+BOOST_AUTO_TEST_CASE ( test_update_placements )
 {
   using namespace Eigen;
   using namespace se3;
@@ -79,7 +79,33 @@ BOOST_AUTO_TEST_CASE ( test_single_kinematics )
   q.middleRows<4> (3).normalize();
 
   forwardKinematics(model, data, q);
-  frameForwardKinematics(model, data, frame_idx);
+  updateFramePlacements(model, data);
+
+  framesForwardKinematics(model, data_ref, q);
+
+  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
+}
+
+BOOST_AUTO_TEST_CASE ( test_update_single_placement )
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  const SE3 & framePlacement = SE3::Random();
+  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
+  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
+  se3::Data data(model);
+  se3::Data data_ref(model);
+
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.middleRows<4> (3).normalize();
+
+  forwardKinematics(model, data, q);
+  updateFramePlacement(model, data, frame_idx);
 
   framesForwardKinematics(model, data_ref, q);
 
@@ -199,9 +225,10 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
   VectorXd a = VectorXd::Random(model.nv);
   
   computeJointJacobiansTimeVariation(model,data,q,v);
+  updateFramePlacements(model,data);
+
   forwardKinematics(model,data_ref,q,v,a);
-  framesForwardKinematics(model,data_ref);
-  framesForwardKinematics(model,data);
+  updateFramePlacements(model,data_ref);  
 
   BOOST_CHECK(isFinite(data.dJ));
 
@@ -250,7 +277,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
     Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
     J_ref_world.fill(0.);     J_ref_local.fill(0.);
     computeJointJacobians(model,data_ref,q);
-    framesForwardKinematics(model,data_ref);
+    updateFramePlacements(model,data_ref);
     const SE3 & oMf_q = data_ref.oMf[idx];
     getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
     getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
@@ -259,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
     Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
     J_ref_plus_world.fill(0.);    J_ref_plus_local.fill(0.);
     computeJointJacobians(model,data_ref_plus,q_plus);
-    framesForwardKinematics(model,data_ref_plus);
+    updateFramePlacements(model,data_ref_plus);
     const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
     getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
     getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
@@ -275,7 +302,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
     //data
     computeJointJacobiansTimeVariation(model,data,q,v);
     forwardKinematics(model,data,q,v);
-    framesForwardKinematics(model,data);
+    updateFramePlacements(model,data);
     Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
     dJ_world.fill(0.);    dJ_local.fill(0.);
     getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world);