Jacobian * Wrench = Joint Torques

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.

/**

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

Ruben Smits's picture

Jacobian * Wrench = Joint Torques

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

Jacobian * Wrench = Joint Torques

On Jun 23, 2011, at 6:55 AM, Ruben Smits wrote:

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

Thanks for the tips, Ruben. I'm new to Eigen. Didn't know Map existed.

--dustin

Jacobian * Wrench = Joint Torques

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>

Jacobian * Wrench = Joint Torques

On Jun 23, 2011, at 03:56 , Herman Bruyninckx wrote:

> 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.

So the "kinematics and dynamics library" part of "Open Robot Control Software" shouldn't contain "operations useful in robot control"?! Seriously!

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

+1 with Peter. I don't see the twist.

One thing I do notice is the use of resize() on JntArray() => possible non-realtime (though the user _should_ have set the correct size before calling, if they're concerned about that).
S

Jacobian * Wrench = Joint Torques

On Thu, 23 Jun 2011, S Roderick wrote:

> On Jun 23, 2011, at 03:56 , Herman Bruyninckx wrote:
>
>> 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.
>
> So the "kinematics and dynamics library" part of "Open Robot Control
> Software" shouldn't contain "operations useful in robot control"?!
> Seriously!

Seriously indeed! I am all in favour of small, (sub)domain-specific extra
libraries, but not of making one big fat one-size-fits-all library with a
mash-up of all utility functions that someone has found useful to have, at
a certain moment in time, in one particular application scope...

I consider "composition of functionality by aggregation" a major design
best practice for infrastructure software. Good example: Linux kernel.

>> And I am a bit surprised to see a "Twist" appearing in the functions,
>> without any reference in the function documentation...?
>
> +1 with Peter. I don't see the twist.

> One thing I do notice is the use of resize() on JntArray() => possible
> non-realtime (though the user _should_ have set the correct size before
> calling, if they're concerned about that).
> S

Herman

Jacobian * Wrench = Joint Torques

On Thu, Jun 23, 2011 at 9:56 AM, Herman Bruyninckx
<Herman [dot] Bruyninckx [..] ...> wrote:
> 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...?

You're probably confused by the context code in the patch of the other
function which Dustin didn't touch.

Peter
--
Orocos-Dev mailing list
Orocos-Dev [..] ...
http://lists.mech.kuleuven.be/mailman/listinfo/orocos-dev

Ruben Smits's picture

Jacobian * Wrench = Joint Torques

On Thursday 24 November 2011 19:47:40 dominick [dot] vanthienen [..] ...
wrote:
> has this been released somewhere?

We have received a patch for this in June:
http://www.orocos.org/forum/rtt/rtt-dev/jacobian-wrench-joint-torques

I did not merge it yet though.

Jacobian * Wrench = Joint Torques

has this been released somewhere?