On 22 Jun 2011, at 19:27, Gooding, Dustin R. (JSC-ER411) wrote:

We will often multiply a Jacobian (transposed) by a Wrench to get Joint Torques. I've added a method, modeled on MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest), that does just that.

I'd appreciate feedback on if this would be useful to others or if there is a more elegant way to achieve the same results.

You should try to avoid any memory allocations, I usually use Eigen::Map as much as possible to achieve this, it also allows you to avoid unnecessary copies.

/**

elenwe:orocos-kdl-svn dgooding$ svn diff
Index: src/jntarray.hpp
===================================================================
--- src/jntarray.hpp (revision 34651)
+++ src/jntarray.hpp (working copy)
@@ -204,6 +204,16 @@
*/
void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
/**
+ * Function to multiply a KDL::Jacobian with a KDL::Wrench
+ * to get a KDL::JntArray of torques.
+ * Jt*w = q
+ *
+ * @param jac Jt
+ * @param src w
+ * @param dest q
+ */
+ void MultiplyJacobian(const Jacobian& jac, const Wrench& src, JntArray& dest);
+ /**
* Function to set all the values of the array to 0
*
* @param array
Index: src/jntarray.cpp
===================================================================
--- src/jntarray.cpp (revision 34651)
+++ src/jntarray.cpp (working copy)
@@ -105,6 +105,27 @@
dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
}

+ void MultiplyJacobian(const Jacobian& jac, const Wrench& src, JntArray& dest)
+ {
+ Eigen::Matrix<double,6,1> w;
+ w(0) = src.force(0);
+ w(1) = src.force(1);
+ w(2) = src.force(2);
+ w(3) = src.torque(0);
+ w(4) = src.torque(1);
+ w(5) = src.torque(2);

or,

w<<(Eigen::Map<Vector3d>(src.force.data),Eigen::Map<Vector3d>(src.torque.data));

+
+ Eigen::MatrixXd j = jac.data.lazy();

+ j.transposeInPlace();
+
+ Eigen::VectorXd t(jac.columns());
+ t = j*w;
+
+ dest.resize(jac.columns());

and why aren't you just doing:

dest.data=(jac.data.tranpose()*w).lazy()

+ for (unsigned i=0; i<jac.columns(); i++)
+ dest(i) = t(i);
+ }
+
void SetToZero(JntArray& array)
{
array.data.setZero();

**/

--
Dustin Gooding

-- Ruben

>

On Wed, 22 Jun 2011, Gooding, Dustin R. (JSC-ER411) wrote:

> We will often multiply a Jacobian (transposed) by a Wrench to get Joint Torques.
>  I've added a method, modeled on MultiplyJacobian(const Jacobian& jac, const
> JntArray& src, Twist& dest), that does just that.

> I'd appreciate feedback on if this would be useful to others or if there is a
> more elegant way to achieve the same results.

These are utility functions that have their merit. But I would put them in
separate libraries, and not in the core. Because there are tons of these,
depending on the kind of stuff that particular application domains use. The
"Jacobian transpose" operations is useful in robot control, but not at all
in many other domains that use KDL.

And I am a bit surprised to see a "Twist" appearing in the functions,
without any reference in the function documentation...?

Herman
>
> /**
>
> elenwe:orocos-kdl-svn dgooding$ svn diff
> Index: src/jntarray.hpp
> ===================================================================
> --- src/jntarray.hpp (revision 34651)
> +++ src/jntarray.hpp (working copy)
> @@ -204,6 +204,16 @@
>       */
>      void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist&
> dest);
>      /**
> +     * Function to multiply a KDL::Jacobian with a KDL::Wrench
> +     * to get a KDL::JntArray of torques.
> +     * Jt*w = q
> +     *
> +     * @param jac Jt
> +     * @param src w
> +     * @param dest q
> +     */
> +    void MultiplyJacobian(const Jacobian& jac, const Wrench& src, JntArray&
> dest);
> +    /**
>       * Function to set all the values of the array to 0
>       *
>       * @param array
> Index: src/jntarray.cpp
> ===================================================================
> --- src/jntarray.cpp (revision 34651)
> +++ src/jntarray.cpp (working copy)
> @@ -105,6 +105,27 @@
>          dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
>      }
>      
> +    void MultiplyJacobian(const Jacobian& jac, const Wrench& src, JntArray&
> dest)
> +    {
> +        Eigen::Matrix<double,6,1> w;
> +        w(0) = src.force(0);
> +        w(1) = src.force(1);
> +        w(2) = src.force(2);
> +        w(3) = src.torque(0);
> +        w(4) = src.torque(1);
> +        w(5) = src.torque(2);
> +        
> +        Eigen::MatrixXd j = jac.data.lazy();
> +        j.transposeInPlace();
> +        
> +        Eigen::VectorXd t(jac.columns());
> +        t = j*w;
> +        
> +        dest.resize(jac.columns());
> +        for (unsigned i=0; i<jac.columns(); i++)
> +         dest(i) = t(i);
> +    }
> +    
>      void SetToZero(JntArray& array)
>      {
>          array.data.setZero();
>
> **/
>
> --
> Dustin Gooding
>
>
>
>

--
K.U.Leuven, Mechanical Eng., Mechatronics & Robotics Research Group
<http://people.mech.kuleuven.be/~bruyninc> Tel: +32 16 328056
EURON Coordinator (European Robotics Research Network) <http://www.euron.org>
Open Realtime Control Services <http://www.orocos.org>
Associate Editor JOSER <http://www.joser.org>, IJRR <http://www.ijrr.org>