iTaSC Cartesian Motion Task

Hello iTaSC users,

I am working with iTaSC for a couple of weeks now, I implemented a
simple example where the first task is to follow a Cartesian trajectory
and the second to avoid the joint limits of our robot. I am using the
existing cartesian_motion and joint_limit_avoidance packages for this.
It is kind of working, but the output of the WDLSPriorVelSolver is very
"noisy" when I activate CC_Cartesian, although the input trajectory is
smooth. I attached a screenshot showing the output of the Solver`s qdot
- port. When I activate the joint limit avoidance alone, the output of
the solver is smooth again.

Now, I also tried the comanipulation demo example and here it's the
same: The Cartesian Motion based tasks produce noisy output, while the
other tasks give smooth output.

Does anyone of you know notice the same thing? Is this behavior of
CC_Cartesian "normal" or am I doing something wrong?

Btw I am using the orocos_toolchain_ros stack.

Best regards,
Dennis

AttachmentSize
screenshot.png52.13 KB

iTaSC Cartesian Motion Task

On 11/09/2012 01:54 PM, S Roderick wrote:
> On Nov 9, 2012, at 07:35 , Dominick Vanthienen wrote:
>
>>
>> On 11/09/2012 10:33 AM, Dennis Mronga wrote:
>>> On 11/08/2012 04:10 PM, Dominick Vanthienen wrote:
>>>>
>>>> On 11/08/2012 11:27 AM, Dennis Mronga wrote:
>>>>> On 11/08/2012 10:40 AM, Ruben Smits wrote:
>>>>>> On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>>>>>>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>>>>>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>>>>>>> wrote:
>>>>>>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>>>>>>> Hi,
>>>>>>>>>>
>>>>>>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>>>>>>
>>>>>>>>>>>> Hello iTaSC users,
>>>>>>>>>>>>
>>>>>>>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>>>>>>>> simple
>>>>>>>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>>>>>>>> the
>>>>>>>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>>>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>>>>>>>> kind
>>>>>>>>>>>> of
>>>>>>>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>>>>>>>> I
>>>>>>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>>>>>>> attached a
>>>>>>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>>>>>>> activate
>>>>>>>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>>>>>>>> again.
>>>>>>>>>>>>
>>>>>>>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>>>>>>>> same: The
>>>>>>>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>>>>>>>> tasks
>>>>>>>>>>>> give
>>>>>>>>>>>> smooth output.
>>>>>>>>>>>>
>>>>>>>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>>>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>>>>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>>>>>>>> qdot (output of the solver)?
>>>>>>>>>> what weights and priorities are you using?
>>>>>>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>>>>>>> reduced
>>>>>>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>>>>>>> motion
>>>>>>>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>>>>>>>> are
>>>>>>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>>>>>>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>>>>>>>> for
>>>>>>>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>>>>>>>> also
>>>>>>>>> attached the .cpf file with the weights and gains. Result is still the
>>>>>>>>> same.
>>>>>>>>>
>>>>>>>>> Any ideas?
>>>>>> Another question,
>>>>>>
>>>>>> is this actually connected to the robot or are you using a simulator,
>>>>>> if so, how are you integrating the output qdots to q's?
>>>>>>
>>>>>> Ruben
>>>>> It is in simulation only, so there should be no noise at all.
>>>>>
>>>>> However, by now I've tested the whole thing on 4 different PCs. All of them have ubuntu 10.04 and ros electric. The only difference is that 2 of the systems are 32 bit and
>>>>> the other 2 are 64 bit architectures. Both, the comanipulation demo and my stuff are working nicely on the 64-bit systems. The problems occur only on the 32-bit systems.
>>>>>
>>>>> Could it be an issue that itasc or one of it's dependencies has to be compiled differently on 32 bit systems?
>>>> Problems are (most likely) occuring in the solver, which has two main dependencies: KDL and Eigen
>>>> I think Eigen is more dependent on the system than KDL itself (which has Eigen under the hood anyway).
>>>> If you run after compiling under Debug, can you pin-point a function/line where the 'jumps' are 'created' (the SVD maybe?)
>>>> Another thing is to increase the Seps and precision properties
>>> I could trace back the problem. The jumps are not created in the solver, but in the Scene.cpp, line 962, where it computes the svd of the feature Jacobian. I already tried
>>> to modify the parameters epsilon and max_iter of svn_eigen_HH(), but that does not help.
>> which branch/tag of itasc_core are you using btw? Since it is about 60 lines further with me.
>>> The feature Jacobian looks something like this:
>>>
>>> [1 0 0 0 -1.20536 -0.00937785
>>> 0 1 0 1.20536 0 -0.165
>>> 0 0 1 0.00937785 0.165 0
>>> 0 0 0 1 0 0
>>> 0 0 0 0 1 0
>>> 0 0 0 0 0 1
>> the jacobian changes over time of course, this one is not singular, but maybe it is close at some point? Although with the VKC_cartesian, I don't expect any (since it uses
>> a full rotation matrix)
>> can you put the Sf matrix (the eigen values) on a port and see how they change over time?
>>
>> Can I conclude (if it is not singular) that svd_eigen_HH of KDL is misbehaving on a 32 bit system?
>> what KDL version are you using?
I am using version 0.23 which comes with ROS electric. Btw I am on the
head revision of itasc.

The singular values look ok. I'll maybe try and use the latest KDL version.

> I don't remember the details, but we've seen different performance on 32- vs 64-bit systems of the SVD_HH in KDL. We never figured it out, but I do remember posting a bug report on it a couple of years back.
> S
>

iTaSC Cartesian Motion Task

On Mon, 5 Nov 2012, Dennis Mronga wrote:

> Hello iTaSC users,
>
> I am working with iTaSC for a couple of weeks now, I implemented a simple
> example where the first task is to follow a Cartesian trajectory and the
> second to avoid the joint limits of our robot. I am using the existing
> cartesian_motion and joint_limit_avoidance packages for this. It is kind of
> working, but the output of the WDLSPriorVelSolver is very "noisy" when I
> activate CC_Cartesian, although the input trajectory is smooth. I attached a
> screenshot showing the output of the Solver`s qdot - port. When I activate
> the joint limit avoidance alone, the output of the solver is smooth again.
>
> Now, I also tried the comanipulation demo example and here it's the same: The
> Cartesian Motion based tasks produce noisy output, while the other tasks give
> smooth output.
>
> Does anyone of you know notice the same thing? Is this behavior of
> CC_Cartesian "normal" or am I doing something wrong?

I do not know what the units are on your plot, but maybe your are zooning
in too hard, with respect to the resolution that is relevant to your task.

Alternatively, you maybe use high control gains in the Cartesian part?
>
> Btw I am using the orocos_toolchain_ros stack.
>
> Best regards,
> Dennis

Herman

iTaSC Cartesian Motion Task

Hi,

On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>
>> Hello iTaSC users,
>>
>> I am working with iTaSC for a couple of weeks now, I implemented a simple
>> example where the first task is to follow a Cartesian trajectory and the
>> second to avoid the joint limits of our robot. I am using the existing
>> cartesian_motion and joint_limit_avoidance packages for this. It is kind of
>> working, but the output of the WDLSPriorVelSolver is very "noisy" when I
>> activate CC_Cartesian, although the input trajectory is smooth. I attached a
>> screenshot showing the output of the Solver`s qdot - port. When I activate
>> the joint limit avoidance alone, the output of the solver is smooth again.
>>
>> Now, I also tried the comanipulation demo example and here it's the same: The
>> Cartesian Motion based tasks produce noisy output, while the other tasks give
>> smooth output.
>>
>> Does anyone of you know notice the same thing? Is this behavior of
>> CC_Cartesian "normal" or am I doing something wrong?

can you say what you are plotting here? ydot or qdot? which DOF?
can you also plot the ydot (input to the solver) for each priority, and qdot (output of the solver)?
what weights and priorities are you using?
>
> I do not know what the units are on your plot, but maybe your are zooning
> in too hard, with respect to the resolution that is relevant to your task.
>
> Alternatively, you maybe use high control gains in the Cartesian part?
>>
>> Btw I am using the orocos_toolchain_ros stack.
>>
>> Best regards,
>> Dennis
>
> Herman
>
nick

iTaSC Cartesian Motion Task

On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
> Hi,
>
> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>
>>> Hello iTaSC users,
>>>
>>> I am working with iTaSC for a couple of weeks now, I implemented a simple
>>> example where the first task is to follow a Cartesian trajectory and the
>>> second to avoid the joint limits of our robot. I am using the existing
>>> cartesian_motion and joint_limit_avoidance packages for this. It is kind of
>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when I
>>> activate CC_Cartesian, although the input trajectory is smooth. I attached a
>>> screenshot showing the output of the Solver`s qdot - port. When I activate
>>> the joint limit avoidance alone, the output of the solver is smooth again.
>>>
>>> Now, I also tried the comanipulation demo example and here it's the same: The
>>> Cartesian Motion based tasks produce noisy output, while the other tasks give
>>> smooth output.
>>>
>>> Does anyone of you know notice the same thing? Is this behavior of
>>> CC_Cartesian "normal" or am I doing something wrong?
> can you say what you are plotting here? ydot or qdot? which DOF?
> can you also plot the ydot (input to the solver) for each priority, and qdot (output of the solver)?
> what weights and priorities are you using?
I attached a plot of ydot (solver input) and qdot (solver output). I
reduced the task to a 6 dof kinematic structure and apply only the
Cartesian motion task, so the plots show all entries of ydot and qdot
repectively. Units are m/s and rad/s for ydot and rad/s for qdot. The
input of the cartesian motion task is given on the DesiredTwist port and
is constant (0.05 m/s) for the x-velocity and 0 for all others. The
priority of the task is 1. I also attached the .cpf file with the
weights and gains. Result is still the same.

Any ideas?

>> I do not know what the units are on your plot, but maybe your are zooning
>> in too hard, with respect to the resolution that is relevant to your task.
>>
>> Alternatively, you maybe use high control gains in the Cartesian part?
>>> Btw I am using the orocos_toolchain_ros stack.
>>>
>>> Best regards,
>>> Dennis
>> Herman
>>
> nick

Ruben Smits's picture

iTaSC Cartesian Motion Task

On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga <dennis [dot] mronga [..] ...> wrote:
> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>
>> Hi,
>>
>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>
>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>
>>>> Hello iTaSC users,
>>>>
>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>> simple
>>>> example where the first task is to follow a Cartesian trajectory and the
>>>> second to avoid the joint limits of our robot. I am using the existing
>>>> cartesian_motion and joint_limit_avoidance packages for this. It is kind
>>>> of
>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when I
>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>> attached a
>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>> activate
>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>> again.
>>>>
>>>> Now, I also tried the comanipulation demo example and here it's the
>>>> same: The
>>>> Cartesian Motion based tasks produce noisy output, while the other tasks
>>>> give
>>>> smooth output.
>>>>
>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>> CC_Cartesian "normal" or am I doing something wrong?
>>
>> can you say what you are plotting here? ydot or qdot? which DOF?
>> can you also plot the ydot (input to the solver) for each priority, and
>> qdot (output of the solver)?
>> what weights and priorities are you using?
>
> I attached a plot of ydot (solver input) and qdot (solver output). I reduced
> the task to a 6 dof kinematic structure and apply only the Cartesian motion
> task, so the plots show all entries of ydot and qdot repectively. Units are
> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
> motion task is given on the DesiredTwist port and is constant (0.05 m/s) for
> the x-velocity and 0 for all others. The priority of the task is 1. I also
> attached the .cpf file with the weights and gains. Result is still the same.
>
> Any ideas?

What is the kinematic structure of your robot like, could it be that
it is close to a singularity? Can you rebuild with Debug information
and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?

Ruben

>
>>> I do not know what the units are on your plot, but maybe your are zooning
>>> in too hard, with respect to the resolution that is relevant to your
>>> task.
>>>
>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>
>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>
>>>> Best regards,
>>>> Dennis
>>>
>>> Herman
>>>
>> nick
>
>
>
> --
> Dennis Mronga
> Researcher
>
> Standort Bremen:
> DFKI GmbH
> Robotics Innovation Center
> Robert-Hooke-Straße 5
> 28359 Bremen, Germany
>
> Phone: +49 (0)421 178 45- 6560
> E-Mail: robotik [..] ...
>
> Weitere Informationen: http://www.dfki.de/robotik
> -----------------------------------------------------------------------
> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
> (Vorsitzender) Dr. Walter Olthoff
> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
> Amtsgericht Kaiserslautern, HRB 2313
> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
> USt-Id.Nr.: DE 148646973
> Steuernummer: 19/673/0060/3
>
>
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>

iTaSC Cartesian Motion Task

On 11/07/2012 03:16 PM, Ruben Smits wrote:
> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>> Hi,
>>>
>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>
>>>>> Hello iTaSC users,
>>>>>
>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>> simple
>>>>> example where the first task is to follow a Cartesian trajectory and the
>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is kind
>>>>> of
>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when I
>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>> attached a
>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>> activate
>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>> again.
>>>>>
>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>> same: The
>>>>> Cartesian Motion based tasks produce noisy output, while the other tasks
>>>>> give
>>>>> smooth output.
>>>>>
>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>> can you also plot the ydot (input to the solver) for each priority, and
>>> qdot (output of the solver)?
>>> what weights and priorities are you using?
>> I attached a plot of ydot (solver input) and qdot (solver output). I reduced
>> the task to a 6 dof kinematic structure and apply only the Cartesian motion
>> task, so the plots show all entries of ydot and qdot repectively. Units are
>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>> motion task is given on the DesiredTwist port and is constant (0.05 m/s) for
>> the x-velocity and 0 for all others. The priority of the task is 1. I also
>> attached the .cpf file with the weights and gains. Result is still the same.
>>
>> Any ideas?
> What is the kinematic structure of your robot like, could it be that
> it is close to a singularity? Can you rebuild with Debug information
> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>
> Ruben

I plotted the output of the S- port and attached it.

>
>>>> I do not know what the units are on your plot, but maybe your are zooning
>>>> in too hard, with respect to the resolution that is relevant to your
>>>> task.
>>>>
>>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>
>>>>> Best regards,
>>>>> Dennis
>>>> Herman
>>>>
>>> nick
>>
>>
>> --
>> Dennis Mronga
>> Researcher
>>
>> Standort Bremen:
>> DFKI GmbH
>> Robotics Innovation Center
>> Robert-Hooke-Straße 5
>> 28359 Bremen, Germany
>>
>> Phone: +49 (0)421 178 45- 6560
>> E-Mail: robotik [..] ...
>>
>> Weitere Informationen: http://www.dfki.de/robotik
>> -----------------------------------------------------------------------
>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>> (Vorsitzender) Dr. Walter Olthoff
>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>> Amtsgericht Kaiserslautern, HRB 2313
>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>> USt-Id.Nr.: DE 148646973
>> Steuernummer: 19/673/0060/3
>>
>>
>> --
>> Orocos-Users mailing list
>> Orocos-Users [..] ...
>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>
>
>

Ruben Smits's picture

iTaSC Cartesian Motion Task

On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga <dennis [dot] mronga [..] ...> wrote:
> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>
>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>> wrote:
>>>
>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>
>>>> Hi,
>>>>
>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>
>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>
>>>>>> Hello iTaSC users,
>>>>>>
>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>> simple
>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>> the
>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>> kind
>>>>>> of
>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>> I
>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>> attached a
>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>> activate
>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>> again.
>>>>>>
>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>> same: The
>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>> tasks
>>>>>> give
>>>>>> smooth output.
>>>>>>
>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>
>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>> qdot (output of the solver)?
>>>> what weights and priorities are you using?
>>>
>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>> reduced
>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>> motion
>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>> are
>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>> for
>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>> also
>>> attached the .cpf file with the weights and gains. Result is still the
>>> same.
>>>
>>> Any ideas?

Another question,

is this actually connected to the robot or are you using a simulator,
if so, how are you integrating the output qdots to q's?

Ruben

>> What is the kinematic structure of your robot like, could it be that
>> it is close to a singularity? Can you rebuild with Debug information
>> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>>
>> Ruben
>
>
> I plotted the output of the S- port and attached it.
>
>
>>
>>>>> I do not know what the units are on your plot, but maybe your are
>>>>> zooning
>>>>> in too hard, with respect to the resolution that is relevant to your
>>>>> task.
>>>>>
>>>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>>>
>>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>>
>>>>>> Best regards,
>>>>>> Dennis
>>>>>
>>>>> Herman
>>>>>
>>>> nick
>>>
>>>
>>>
>>> --
>>> Dennis Mronga
>>> Researcher
>>>
>>> Standort Bremen:
>>> DFKI GmbH
>>> Robotics Innovation Center
>>> Robert-Hooke-Straße 5
>>> 28359 Bremen, Germany
>>>
>>> Phone: +49 (0)421 178 45- 6560
>>> E-Mail: robotik [..] ...
>>>
>>> Weitere Informationen: http://www.dfki.de/robotik
>>> -----------------------------------------------------------------------
>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>> (Vorsitzender) Dr. Walter Olthoff
>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>> Amtsgericht Kaiserslautern, HRB 2313
>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>> USt-Id.Nr.: DE 148646973
>>> Steuernummer: 19/673/0060/3
>>>
>>>
>>> --
>>> Orocos-Users mailing list
>>> Orocos-Users [..] ...
>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>
>>
>>
>
>
> --
> Dennis Mronga
> Researcher
>
> Standort Bremen:
> DFKI GmbH
> Robotics Innovation Center
> Robert-Hooke-Straße 5
> 28359 Bremen, Germany
>
> Phone: +49 (0)421 178 45- 6560
> E-Mail: robotik [..] ...
>
> Weitere Informationen: http://www.dfki.de/robotik
> -----------------------------------------------------------------------
> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
> (Vorsitzender) Dr. Walter Olthoff
> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
> Amtsgericht Kaiserslautern, HRB 2313
> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
> USt-Id.Nr.: DE 148646973
> Steuernummer: 19/673/0060/3
>

iTaSC Cartesian Motion Task

On 11/08/2012 10:40 AM, Ruben Smits wrote:
> On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>> wrote:
>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>> Hi,
>>>>>
>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>
>>>>>>> Hello iTaSC users,
>>>>>>>
>>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>>> simple
>>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>>> the
>>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>>> kind
>>>>>>> of
>>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>>> I
>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>> attached a
>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>> activate
>>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>>> again.
>>>>>>>
>>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>>> same: The
>>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>>> tasks
>>>>>>> give
>>>>>>> smooth output.
>>>>>>>
>>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>>> qdot (output of the solver)?
>>>>> what weights and priorities are you using?
>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>> reduced
>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>> motion
>>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>>> are
>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>>> for
>>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>>> also
>>>> attached the .cpf file with the weights and gains. Result is still the
>>>> same.
>>>>
>>>> Any ideas?
> Another question,
>
> is this actually connected to the robot or are you using a simulator,
> if so, how are you integrating the output qdots to q's?
>
> Ruben

It is in simulation only, so there should be no noise at all.

However, by now I've tested the whole thing on 4 different PCs. All of
them have ubuntu 10.04 and ros electric. The only difference is that 2
of the systems are 32 bit and the other 2 are 64 bit architectures.
Both, the comanipulation demo and my stuff are working nicely on the
64-bit systems. The problems occur only on the 32-bit systems.

Could it be an issue that itasc or one of it's dependencies has to be
compiled differently on 32 bit systems?

Dennis
>
>
>>> What is the kinematic structure of your robot like, could it be that
>>> it is close to a singularity? Can you rebuild with Debug information
>>> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>>>
>>> Ruben
>>
>> I plotted the output of the S- port and attached it.
>>
>>
>>>>>> I do not know what the units are on your plot, but maybe your are
>>>>>> zooning
>>>>>> in too hard, with respect to the resolution that is relevant to your
>>>>>> task.
>>>>>>
>>>>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>>>
>>>>>>> Best regards,
>>>>>>> Dennis
>>>>>> Herman
>>>>>>
>>>>> nick
>>>>
>>>>
>>>> --
>>>> Dennis Mronga
>>>> Researcher
>>>>
>>>> Standort Bremen:
>>>> DFKI GmbH
>>>> Robotics Innovation Center
>>>> Robert-Hooke-Straße 5
>>>> 28359 Bremen, Germany
>>>>
>>>> Phone: +49 (0)421 178 45- 6560
>>>> E-Mail: robotik [..] ...
>>>>
>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>> -----------------------------------------------------------------------
>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>> (Vorsitzender) Dr. Walter Olthoff
>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>> USt-Id.Nr.: DE 148646973
>>>> Steuernummer: 19/673/0060/3
>>>>
>>>>
>>>> --
>>>> Orocos-Users mailing list
>>>> Orocos-Users [..] ...
>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>
>>>
>>
>> --
>> Dennis Mronga
>> Researcher
>>
>> Standort Bremen:
>> DFKI GmbH
>> Robotics Innovation Center
>> Robert-Hooke-Straße 5
>> 28359 Bremen, Germany
>>
>> Phone: +49 (0)421 178 45- 6560
>> E-Mail: robotik [..] ...
>>
>> Weitere Informationen: http://www.dfki.de/robotik
>> -----------------------------------------------------------------------
>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>> (Vorsitzender) Dr. Walter Olthoff
>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>> Amtsgericht Kaiserslautern, HRB 2313
>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>> USt-Id.Nr.: DE 148646973
>> Steuernummer: 19/673/0060/3
>>
>
>

iTaSC Cartesian Motion Task

On 11/08/2012 11:27 AM, Dennis Mronga wrote:
> On 11/08/2012 10:40 AM, Ruben Smits wrote:
>> On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>>> wrote:
>>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>>> Hi,
>>>>>>
>>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>>
>>>>>>>> Hello iTaSC users,
>>>>>>>>
>>>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>>>> simple
>>>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>>>> the
>>>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>>>> kind
>>>>>>>> of
>>>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>>>> I
>>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>>> attached a
>>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>>> activate
>>>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>>>> again.
>>>>>>>>
>>>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>>>> same: The
>>>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>>>> tasks
>>>>>>>> give
>>>>>>>> smooth output.
>>>>>>>>
>>>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>>>> qdot (output of the solver)?
>>>>>> what weights and priorities are you using?
>>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>>> reduced
>>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>>> motion
>>>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>>>> are
>>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>>>> for
>>>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>>>> also
>>>>> attached the .cpf file with the weights and gains. Result is still the
>>>>> same.
>>>>>
>>>>> Any ideas?
>> Another question,
>>
>> is this actually connected to the robot or are you using a simulator,
>> if so, how are you integrating the output qdots to q's?
>>
>> Ruben
>
> It is in simulation only, so there should be no noise at all.
>
> However, by now I've tested the whole thing on 4 different PCs. All of them have ubuntu 10.04 and ros electric. The only difference is that 2 of the systems are 32 bit and
> the other 2 are 64 bit architectures. Both, the comanipulation demo and my stuff are working nicely on the 64-bit systems. The problems occur only on the 32-bit systems.
>
> Could it be an issue that itasc or one of it's dependencies has to be compiled differently on 32 bit systems?

Problems are (most likely) occuring in the solver, which has two main dependencies: KDL and Eigen
I think Eigen is more dependent on the system than KDL itself (which has Eigen under the hood anyway).
If you run after compiling under Debug, can you pin-point a function/line where the 'jumps' are 'created' (the SVD maybe?)
Another thing is to increase the Seps and precision properties
>
> Dennis
>>
>>
>>>> What is the kinematic structure of your robot like, could it be that
>>>> it is close to a singularity? Can you rebuild with Debug information
>>>> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>>>>
>>>> Ruben
>>>
>>> I plotted the output of the S- port and attached it.
>>>
>>>
>>>>>>> I do not know what the units are on your plot, but maybe your are
>>>>>>> zooning
>>>>>>> in too hard, with respect to the resolution that is relevant to your
>>>>>>> task.
>>>>>>>
>>>>>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>>>>
>>>>>>>> Best regards,
>>>>>>>> Dennis
>>>>>>> Herman
>>>>>>>
>>>>>> nick
>>>>>
>>>>>
>>>>> --
>>>>> Dennis Mronga
>>>>> Researcher
>>>>>
>>>>> Standort Bremen:
>>>>> DFKI GmbH
>>>>> Robotics Innovation Center
>>>>> Robert-Hooke-Straße 5
>>>>> 28359 Bremen, Germany
>>>>>
>>>>> Phone: +49 (0)421 178 45- 6560
>>>>> E-Mail: robotik [..] ...
>>>>>
>>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>>> -----------------------------------------------------------------------
>>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>>> (Vorsitzender) Dr. Walter Olthoff
>>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>>> USt-Id.Nr.: DE 148646973
>>>>> Steuernummer: 19/673/0060/3
>>>>>
>>>>>
>>>>> --
>>>>> Orocos-Users mailing list
>>>>> Orocos-Users [..] ...
>>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>>
>>>>
>>>
>>> --
>>> Dennis Mronga
>>> Researcher
>>>
>>> Standort Bremen:
>>> DFKI GmbH
>>> Robotics Innovation Center
>>> Robert-Hooke-Straße 5
>>> 28359 Bremen, Germany
>>>
>>> Phone: +49 (0)421 178 45- 6560
>>> E-Mail: robotik [..] ...
>>>
>>> Weitere Informationen: http://www.dfki.de/robotik
>>> -----------------------------------------------------------------------
>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>> (Vorsitzender) Dr. Walter Olthoff
>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>> Amtsgericht Kaiserslautern, HRB 2313
>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>> USt-Id.Nr.: DE 148646973
>>> Steuernummer: 19/673/0060/3
>>>
>>
>>
>
>

iTaSC Cartesian Motion Task

On 11/08/2012 04:10 PM, Dominick Vanthienen wrote:
>
>
> On 11/08/2012 11:27 AM, Dennis Mronga wrote:
>> On 11/08/2012 10:40 AM, Ruben Smits wrote:
>>> On Thu, Nov 8, 2012 at 10:18 AM, Dennis
>>> Mronga<dennis [dot] mronga [..] ...> wrote:
>>>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>>>> wrote:
>>>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>>>> Hi,
>>>>>>>
>>>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>>>
>>>>>>>>> Hello iTaSC users,
>>>>>>>>>
>>>>>>>>> I am working with iTaSC for a couple of weeks now, I
>>>>>>>>> implemented a
>>>>>>>>> simple
>>>>>>>>> example where the first task is to follow a Cartesian
>>>>>>>>> trajectory and
>>>>>>>>> the
>>>>>>>>> second to avoid the joint limits of our robot. I am using the
>>>>>>>>> existing
>>>>>>>>> cartesian_motion and joint_limit_avoidance packages for this.
>>>>>>>>> It is
>>>>>>>>> kind
>>>>>>>>> of
>>>>>>>>> working, but the output of the WDLSPriorVelSolver is very
>>>>>>>>> "noisy" when
>>>>>>>>> I
>>>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>>>> attached a
>>>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>>>> activate
>>>>>>>>> the joint limit avoidance alone, the output of the solver is
>>>>>>>>> smooth
>>>>>>>>> again.
>>>>>>>>>
>>>>>>>>> Now, I also tried the comanipulation demo example and here
>>>>>>>>> it's the
>>>>>>>>> same: The
>>>>>>>>> Cartesian Motion based tasks produce noisy output, while the
>>>>>>>>> other
>>>>>>>>> tasks
>>>>>>>>> give
>>>>>>>>> smooth output.
>>>>>>>>>
>>>>>>>>> Does anyone of you know notice the same thing? Is this
>>>>>>>>> behavior of
>>>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>>>> can you also plot the ydot (input to the solver) for each
>>>>>>> priority, and
>>>>>>> qdot (output of the solver)?
>>>>>>> what weights and priorities are you using?
>>>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>>>> reduced
>>>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>>>> motion
>>>>>> task, so the plots show all entries of ydot and qdot repectively.
>>>>>> Units
>>>>>> are
>>>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the
>>>>>> cartesian
>>>>>> motion task is given on the DesiredTwist port and is constant
>>>>>> (0.05 m/s)
>>>>>> for
>>>>>> the x-velocity and 0 for all others. The priority of the task
>>>>>> is 1. I
>>>>>> also
>>>>>> attached the .cpf file with the weights and gains. Result is
>>>>>> still the
>>>>>> same.
>>>>>>
>>>>>> Any ideas?
>>> Another question,
>>>
>>> is this actually connected to the robot or are you using a simulator,
>>> if so, how are you integrating the output qdots to q's?
>>>
>>> Ruben
>>
>> It is in simulation only, so there should be no noise at all.
>>
>> However, by now I've tested the whole thing on 4 different PCs. All
>> of them have ubuntu 10.04 and ros electric. The only difference is
>> that 2 of the systems are 32 bit and
>> the other 2 are 64 bit architectures. Both, the comanipulation demo
>> and my stuff are working nicely on the 64-bit systems. The problems
>> occur only on the 32-bit systems.
>>
>> Could it be an issue that itasc or one of it's dependencies has to be
>> compiled differently on 32 bit systems?
>
> Problems are (most likely) occuring in the solver, which has two main
> dependencies: KDL and Eigen
> I think Eigen is more dependent on the system than KDL itself (which
> has Eigen under the hood anyway).
> If you run after compiling under Debug, can you pin-point a
> function/line where the 'jumps' are 'created' (the SVD maybe?)
> Another thing is to increase the Seps and precision properties

I could trace back the problem. The jumps are not created in the solver,
but in the Scene.cpp, line 962, where it computes the svd of the feature
Jacobian. I already tried to modify the parameters epsilon and max_iter
of svn_eigen_HH(), but that does not help.

The feature Jacobian looks something like this:

[1 0 0 0 -1.20536 -0.00937785
0 1 0 1.20536 0 -0.165
0 0 1 0.00937785 0.165 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1

Greetings,
Dennis

>>
>> Dennis
>>>
>>>
>>>>> What is the kinematic structure of your robot like, could it be that
>>>>> it is close to a singularity? Can you rebuild with Debug information
>>>>> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>>>>>
>>>>> Ruben
>>>>
>>>> I plotted the output of the S- port and attached it.
>>>>
>>>>
>>>>>>>> I do not know what the units are on your plot, but maybe your are
>>>>>>>> zooning
>>>>>>>> in too hard, with respect to the resolution that is relevant to
>>>>>>>> your
>>>>>>>> task.
>>>>>>>>
>>>>>>>> Alternatively, you maybe use high control gains in the
>>>>>>>> Cartesian part?
>>>>>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>>>>>
>>>>>>>>> Best regards,
>>>>>>>>> Dennis
>>>>>>>> Herman
>>>>>>>>
>>>>>>> nick
>>>>>>
>>>>>>
>>>>>> --
>>>>>> Dennis Mronga
>>>>>> Researcher
>>>>>>
>>>>>> Standort Bremen:
>>>>>> DFKI GmbH
>>>>>> Robotics Innovation Center
>>>>>> Robert-Hooke-Straße 5
>>>>>> 28359 Bremen, Germany
>>>>>>
>>>>>> Phone: +49 (0)421 178 45- 6560
>>>>>> E-Mail: robotik [..] ...
>>>>>>
>>>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>>>> -----------------------------------------------------------------------
>>>>>>
>>>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>>>> (Vorsitzender) Dr. Walter Olthoff
>>>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>>>> USt-Id.Nr.: DE 148646973
>>>>>> Steuernummer: 19/673/0060/3
>>>>>>
>>>>>>
>>>>>> --
>>>>>> Orocos-Users mailing list
>>>>>> Orocos-Users [..] ...
>>>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>>>
>>>>>
>>>>
>>>> --
>>>> Dennis Mronga
>>>> Researcher
>>>>
>>>> Standort Bremen:
>>>> DFKI GmbH
>>>> Robotics Innovation Center
>>>> Robert-Hooke-Straße 5
>>>> 28359 Bremen, Germany
>>>>
>>>> Phone: +49 (0)421 178 45- 6560
>>>> E-Mail: robotik [..] ...
>>>>
>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>> -----------------------------------------------------------------------
>>>>
>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>> (Vorsitzender) Dr. Walter Olthoff
>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>> USt-Id.Nr.: DE 148646973
>>>> Steuernummer: 19/673/0060/3
>>>>
>>>
>>>
>>
>>

iTaSC Cartesian Motion Task

On 11/09/2012 10:33 AM, Dennis Mronga wrote:
> On 11/08/2012 04:10 PM, Dominick Vanthienen wrote:
>>
>>
>> On 11/08/2012 11:27 AM, Dennis Mronga wrote:
>>> On 11/08/2012 10:40 AM, Ruben Smits wrote:
>>>> On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>>>>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>>>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>>>>> wrote:
>>>>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>>>>> Hi,
>>>>>>>>
>>>>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>>>>
>>>>>>>>>> Hello iTaSC users,
>>>>>>>>>>
>>>>>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>>>>>> simple
>>>>>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>>>>>> the
>>>>>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>>>>>> kind
>>>>>>>>>> of
>>>>>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>>>>>> I
>>>>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>>>>> attached a
>>>>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>>>>> activate
>>>>>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>>>>>> again.
>>>>>>>>>>
>>>>>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>>>>>> same: The
>>>>>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>>>>>> tasks
>>>>>>>>>> give
>>>>>>>>>> smooth output.
>>>>>>>>>>
>>>>>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>>>>>> qdot (output of the solver)?
>>>>>>>> what weights and priorities are you using?
>>>>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>>>>> reduced
>>>>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>>>>> motion
>>>>>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>>>>>> are
>>>>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>>>>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>>>>>> for
>>>>>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>>>>>> also
>>>>>>> attached the .cpf file with the weights and gains. Result is still the
>>>>>>> same.
>>>>>>>
>>>>>>> Any ideas?
>>>> Another question,
>>>>
>>>> is this actually connected to the robot or are you using a simulator,
>>>> if so, how are you integrating the output qdots to q's?
>>>>
>>>> Ruben
>>>
>>> It is in simulation only, so there should be no noise at all.
>>>
>>> However, by now I've tested the whole thing on 4 different PCs. All of them have ubuntu 10.04 and ros electric. The only difference is that 2 of the systems are 32 bit and
>>> the other 2 are 64 bit architectures. Both, the comanipulation demo and my stuff are working nicely on the 64-bit systems. The problems occur only on the 32-bit systems.
>>>
>>> Could it be an issue that itasc or one of it's dependencies has to be compiled differently on 32 bit systems?
>>
>> Problems are (most likely) occuring in the solver, which has two main dependencies: KDL and Eigen
>> I think Eigen is more dependent on the system than KDL itself (which has Eigen under the hood anyway).
>> If you run after compiling under Debug, can you pin-point a function/line where the 'jumps' are 'created' (the SVD maybe?)
>> Another thing is to increase the Seps and precision properties
>
> I could trace back the problem. The jumps are not created in the solver, but in the Scene.cpp, line 962, where it computes the svd of the feature Jacobian. I already tried
> to modify the parameters epsilon and max_iter of svn_eigen_HH(), but that does not help.
which branch/tag of itasc_core are you using btw? Since it is about 60 lines further with me.
>
> The feature Jacobian looks something like this:
>
> [1 0 0 0 -1.20536 -0.00937785
> 0 1 0 1.20536 0 -0.165
> 0 0 1 0.00937785 0.165 0
> 0 0 0 1 0 0
> 0 0 0 0 1 0
> 0 0 0 0 0 1
the jacobian changes over time of course, this one is not singular, but maybe it is close at some point? Although with the VKC_cartesian, I don't expect any (since it uses
a full rotation matrix)
can you put the Sf matrix (the eigen values) on a port and see how they change over time?

Can I conclude (if it is not singular) that svd_eigen_HH of KDL is misbehaving on a 32 bit system?
what KDL version are you using?

>
> Greetings,
> Dennis
>
>>>
>>> Dennis
>>>>
>>>>
>>>>>> What is the kinematic structure of your robot like, could it be that
>>>>>> it is close to a singularity? Can you rebuild with Debug information
>>>>>> and run with ORO_LOGLEVEL=7 (Debug), and check the singular values?
>>>>>>
>>>>>> Ruben
>>>>>
>>>>> I plotted the output of the S- port and attached it.
>>>>>
>>>>>
>>>>>>>>> I do not know what the units are on your plot, but maybe your are
>>>>>>>>> zooning
>>>>>>>>> in too hard, with respect to the resolution that is relevant to your
>>>>>>>>> task.
>>>>>>>>>
>>>>>>>>> Alternatively, you maybe use high control gains in the Cartesian part?
>>>>>>>>>> Btw I am using the orocos_toolchain_ros stack.
>>>>>>>>>>
>>>>>>>>>> Best regards,
>>>>>>>>>> Dennis
>>>>>>>>> Herman
>>>>>>>>>
>>>>>>>> nick
>>>>>>>
>>>>>>>
>>>>>>> --
>>>>>>> Dennis Mronga
>>>>>>> Researcher
>>>>>>>
>>>>>>> Standort Bremen:
>>>>>>> DFKI GmbH
>>>>>>> Robotics Innovation Center
>>>>>>> Robert-Hooke-Straße 5
>>>>>>> 28359 Bremen, Germany
>>>>>>>
>>>>>>> Phone: +49 (0)421 178 45- 6560
>>>>>>> E-Mail: robotik [..] ...
>>>>>>>
>>>>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>>>>> -----------------------------------------------------------------------
>>>>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>>>>> (Vorsitzender) Dr. Walter Olthoff
>>>>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>>>>> USt-Id.Nr.: DE 148646973
>>>>>>> Steuernummer: 19/673/0060/3
>>>>>>>
>>>>>>>
>>>>>>> --
>>>>>>> Orocos-Users mailing list
>>>>>>> Orocos-Users [..] ...
>>>>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>>>>
>>>>>>
>>>>>
>>>>> --
>>>>> Dennis Mronga
>>>>> Researcher
>>>>>
>>>>> Standort Bremen:
>>>>> DFKI GmbH
>>>>> Robotics Innovation Center
>>>>> Robert-Hooke-Straße 5
>>>>> 28359 Bremen, Germany
>>>>>
>>>>> Phone: +49 (0)421 178 45- 6560
>>>>> E-Mail: robotik [..] ...
>>>>>
>>>>> Weitere Informationen: http://www.dfki.de/robotik
>>>>> -----------------------------------------------------------------------
>>>>> Deutsches Forschungszentrum fuer Kuenstliche Intelligenz GmbH
>>>>> Firmensitz: Trippstadter Straße 122, D-67663 Kaiserslautern
>>>>> Geschaeftsfuehrung: Prof. Dr. Dr. h.c. mult. Wolfgang Wahlster
>>>>> (Vorsitzender) Dr. Walter Olthoff
>>>>> Vorsitzender des Aufsichtsrats: Prof. Dr. h.c. Hans A. Aukes
>>>>> Amtsgericht Kaiserslautern, HRB 2313
>>>>> Sitz der Gesellschaft: Kaiserslautern (HRB 2313)
>>>>> USt-Id.Nr.: DE 148646973
>>>>> Steuernummer: 19/673/0060/3
>>>>>
>>>>
>>>>
>>>
>>>
>
>

iTaSC Cartesian Motion Task

On Nov 9, 2012, at 07:35 , Dominick Vanthienen wrote:

>
>
> On 11/09/2012 10:33 AM, Dennis Mronga wrote:
>> On 11/08/2012 04:10 PM, Dominick Vanthienen wrote:
>>>
>>>
>>> On 11/08/2012 11:27 AM, Dennis Mronga wrote:
>>>> On 11/08/2012 10:40 AM, Ruben Smits wrote:
>>>>> On Thu, Nov 8, 2012 at 10:18 AM, Dennis Mronga<dennis [dot] mronga [..] ...> wrote:
>>>>>> On 11/07/2012 03:16 PM, Ruben Smits wrote:
>>>>>>> On Wed, Nov 7, 2012 at 10:37 AM, Dennis Mronga<dennis [dot] mronga [..] ...>
>>>>>>> wrote:
>>>>>>>> On 11/06/2012 01:23 PM, Dominick Vanthienen wrote:
>>>>>>>>> Hi,
>>>>>>>>>
>>>>>>>>> On 11/05/2012 02:45 PM, Herman Bruyninckx wrote:
>>>>>>>>>> On Mon, 5 Nov 2012, Dennis Mronga wrote:
>>>>>>>>>>
>>>>>>>>>>> Hello iTaSC users,
>>>>>>>>>>>
>>>>>>>>>>> I am working with iTaSC for a couple of weeks now, I implemented a
>>>>>>>>>>> simple
>>>>>>>>>>> example where the first task is to follow a Cartesian trajectory and
>>>>>>>>>>> the
>>>>>>>>>>> second to avoid the joint limits of our robot. I am using the existing
>>>>>>>>>>> cartesian_motion and joint_limit_avoidance packages for this. It is
>>>>>>>>>>> kind
>>>>>>>>>>> of
>>>>>>>>>>> working, but the output of the WDLSPriorVelSolver is very "noisy" when
>>>>>>>>>>> I
>>>>>>>>>>> activate CC_Cartesian, although the input trajectory is smooth. I
>>>>>>>>>>> attached a
>>>>>>>>>>> screenshot showing the output of the Solver`s qdot - port. When I
>>>>>>>>>>> activate
>>>>>>>>>>> the joint limit avoidance alone, the output of the solver is smooth
>>>>>>>>>>> again.
>>>>>>>>>>>
>>>>>>>>>>> Now, I also tried the comanipulation demo example and here it's the
>>>>>>>>>>> same: The
>>>>>>>>>>> Cartesian Motion based tasks produce noisy output, while the other
>>>>>>>>>>> tasks
>>>>>>>>>>> give
>>>>>>>>>>> smooth output.
>>>>>>>>>>>
>>>>>>>>>>> Does anyone of you know notice the same thing? Is this behavior of
>>>>>>>>>>> CC_Cartesian "normal" or am I doing something wrong?
>>>>>>>>> can you say what you are plotting here? ydot or qdot? which DOF?
>>>>>>>>> can you also plot the ydot (input to the solver) for each priority, and
>>>>>>>>> qdot (output of the solver)?
>>>>>>>>> what weights and priorities are you using?
>>>>>>>> I attached a plot of ydot (solver input) and qdot (solver output). I
>>>>>>>> reduced
>>>>>>>> the task to a 6 dof kinematic structure and apply only the Cartesian
>>>>>>>> motion
>>>>>>>> task, so the plots show all entries of ydot and qdot repectively. Units
>>>>>>>> are
>>>>>>>> m/s and rad/s for ydot and rad/s for qdot. The input of the cartesian
>>>>>>>> motion task is given on the DesiredTwist port and is constant (0.05 m/s)
>>>>>>>> for
>>>>>>>> the x-velocity and 0 for all others. The priority of the task is 1. I
>>>>>>>> also
>>>>>>>> attached the .cpf file with the weights and gains. Result is still the
>>>>>>>> same.
>>>>>>>>
>>>>>>>> Any ideas?
>>>>> Another question,
>>>>>
>>>>> is this actually connected to the robot or are you using a simulator,
>>>>> if so, how are you integrating the output qdots to q's?
>>>>>
>>>>> Ruben
>>>>
>>>> It is in simulation only, so there should be no noise at all.
>>>>
>>>> However, by now I've tested the whole thing on 4 different PCs. All of them have ubuntu 10.04 and ros electric. The only difference is that 2 of the systems are 32 bit and
>>>> the other 2 are 64 bit architectures. Both, the comanipulation demo and my stuff are working nicely on the 64-bit systems. The problems occur only on the 32-bit systems.
>>>>
>>>> Could it be an issue that itasc or one of it's dependencies has to be compiled differently on 32 bit systems?
>>>
>>> Problems are (most likely) occuring in the solver, which has two main dependencies: KDL and Eigen
>>> I think Eigen is more dependent on the system than KDL itself (which has Eigen under the hood anyway).
>>> If you run after compiling under Debug, can you pin-point a function/line where the 'jumps' are 'created' (the SVD maybe?)
>>> Another thing is to increase the Seps and precision properties
>>
>> I could trace back the problem. The jumps are not created in the solver, but in the Scene.cpp, line 962, where it computes the svd of the feature Jacobian. I already tried
>> to modify the parameters epsilon and max_iter of svn_eigen_HH(), but that does not help.
> which branch/tag of itasc_core are you using btw? Since it is about 60 lines further with me.
>>
>> The feature Jacobian looks something like this:
>>
>> [1 0 0 0 -1.20536 -0.00937785
>> 0 1 0 1.20536 0 -0.165
>> 0 0 1 0.00937785 0.165 0
>> 0 0 0 1 0 0
>> 0 0 0 0 1 0
>> 0 0 0 0 0 1
> the jacobian changes over time of course, this one is not singular, but maybe it is close at some point? Although with the VKC_cartesian, I don't expect any (since it uses
> a full rotation matrix)
> can you put the Sf matrix (the eigen values) on a port and see how they change over time?
>
> Can I conclude (if it is not singular) that svd_eigen_HH of KDL is misbehaving on a 32 bit system?
> what KDL version are you using?

I don't remember the details, but we've seen different performance on 32- vs 64-bit systems of the SVD_HH in KDL. We never figured it out, but I do remember posting a bug report on it a couple of years back.
S