interruptible moveTo

Hi,

We currently use the moveTo command of OCL::CartesianGeneratorPos
(1.1) to move our lwr.

1. Now, between the commanded cartesian positions the robot does a
stop and go motion. Is there any components available that does a
continuous movement between the commanded points ? something like the
continuous PTP of KRL. (we basically need an interruptible moveTo
command.)

[In http://svn.mech.kuleuven.be/repos/orocos/trunk/kul-ros-pkg/pr2_lwr_coupl...
the author writes the arriving ROS TF frame directly to the
FRIComponent. This was not possible to us unless the commanded and the
measured postions were close. Otherwise the robot hits velocity
limits. Therefore we need an interpolator in between the friComponent
and our custom component that gives out desired Cartesian positions]

2. Where can we find the OCL::CartesianGeneratorPos for 2.0 ?

Thankyou,
Gajan

interruptible moveTo

On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:

> We currently use the moveTo command of OCL::CartesianGeneratorPos
> (1.1) to move our lwr.
>
> 1. Now, between the commanded cartesian positions the robot does a
> stop and go motion. Is there any components available that does a
> continuous movement between the commanded points ? something like the
> continuous PTP of KRL. (we basically need an interruptible moveTo
> command.)

There is no such implementation currently provided. I personally made a
matlab version of such a thing some years ago, but I have never published
it because (i) the paper that I wrote about it was never accepted, (ii) it
was not a closed-form solution and needed some numerical iterations, and
(iii) it is not that useful really when you are doing sensor-based robot
motions (as is our core activity in Leuven). For the latter, we use our
constrained-based motion generation, ("ITaSC"), as published here
@Article{ deschutter2006,
author = {De~Schutter, Joris and De~Laet, Tinne and
Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben and
Aertbeli\"en, Erwin and Claes, Kasper and
Bruyninckx, Herman},
title = {Constraint-Based Task Specification and Estimation
for Sensor-Based Robot Systems in the Presence of
Geometric Uncertainty},
journal = ijrr,
volume = {26},
number = {5},
pages = {433--455},
year = {2007}
}

and for which a 2.x implementation will be ready "soon".

I am interested in discussing the design of a "blended PTP" trajectory
generator with you, on this mailinglist, in case you are motivated to work
on such a thing.
Torsten Kroeger
<http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
has the most complete version of such a thing, but its solution is rather
complex. And again, overkill in sensor-based tasks.

> [In http://svn.mech.kuleuven.be/repos/orocos/trunk/kul-ros-pkg/pr2_lwr_coupl...
> the author writes the arriving ROS TF frame directly to the
> FRIComponent. This was not possible to us unless the commanded and the
> measured postions were close. Otherwise the robot hits velocity
> limits.

This is, in my opinion, the number one problem with the current FRI: it
does not warn you a bit in advance that it is going to hit its limits :-(

> Therefore we need an interpolator in between the friComponent
> and our custom component that gives out desired Cartesian positions]
>
> 2. Where can we find the OCL::CartesianGeneratorPos for 2.0 ?
>
> Thankyou,
> Gajan

Herman

gajan's picture

interruptible moveTo

Hi Herman,

> There is no such implementation currently provided. I personally made a
> matlab version of such a thing some years ago, but I have never published
> it because (i) the paper that I wrote about it was never accepted, (ii) it
> was not a closed-form solution and needed some numerical iterations, and
> (iii) it is not that useful really when you are doing sensor-based robot
> motions (as is our core activity in Leuven). For the latter, we use our
> constrained-based motion generation, ("ITaSC"), as published here
> @Article{    deschutter2006,
>  author          = {De~Schutter, Joris and De~Laet, Tinne and
>                     Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben and
>                     Aertbeli\"en, Erwin and Claes, Kasper and
>                     Bruyninckx, Herman},
>  title           = {Constraint-Based Task Specification and Estimation
>                     for Sensor-Based Robot Systems in the Presence of
> Geometric Uncertainty},
>  journal         = ijrr,
>  volume          = {26},
>  number          = {5},
>  pages           = {433--455},
>  year            = {2007}
> }
>
> and for which a 2.x implementation will be ready "soon".
Looking forward to it.

>
> I am interested in discussing the design of a "blended PTP" trajectory
> generator with you, on this mailinglist, in case you are motivated to work
> on such a thing.
> Torsten Kroeger
> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
> has the most complete version of such a thing, but its solution is rather
> complex. And again, overkill in sensor-based tasks.
>
I am definitely interested in implementing a blended PTP trajectory generator.
I would like to know which type of OTG ( Fig 3 of [1] ) you have in
mind (given the current FRI ).
Furthermore, I assume we do a time synchronized, kinematic time optimization.
[1] @article{Kroeger:10,
author = {T.~Kr{\"o}ger and F.~M.~Wahl},
title = {On-Line Trajectory Generation: {B}asic Concepts for
Instantaneous Reactions to Unforeseen Events},
journal = {IEEE Trans. on Robotics},
month = {feb},
volume = {26},
number = {1},
pages = {94--111},
year = {2010}
}

Can this problem can be formulated as an MPC (Model Predictive
Control) problem? May be MPC is computationally too expensive in this
case ?

Best Regards,
Gajan

interruptible moveTo

On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:

> Hi Herman,
>
>> There is no such implementation currently provided. I personally made a
>> matlab version of such a thing some years ago, but I have never published
>> it because (i) the paper that I wrote about it was never accepted, (ii) it
>> was not a closed-form solution and needed some numerical iterations, and
>> (iii) it is not that useful really when you are doing sensor-based robot
>> motions (as is our core activity in Leuven). For the latter, we use our
>> constrained-based motion generation, ("ITaSC"), as published here
>> @Article{    deschutter2006,
>>  author          = {De~Schutter, Joris and De~Laet, Tinne and
>>                     Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben and
>>                     Aertbeli\"en, Erwin and Claes, Kasper and
>>                     Bruyninckx, Herman},
>>  title           = {Constraint-Based Task Specification and Estimation
>>                     for Sensor-Based Robot Systems in the Presence of
>> Geometric Uncertainty},
>>  journal         = ijrr,
>>  volume          = {26},
>>  number          = {5},
>>  pages           = {433--455},
>>  year            = {2007}
>> }
>>
>> and for which a 2.x implementation will be ready "soon".
> Looking forward to it.
>
>> I am interested in discussing the design of a "blended PTP" trajectory
>> generator with you, on this mailinglist, in case you are motivated to work
>> on such a thing.
>> Torsten Kroeger
>> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
>> has the most complete version of such a thing, but its solution is rather
>> complex. And again, overkill in sensor-based tasks.
>>
> I am definitely interested in implementing a blended PTP trajectory generator.
> I would like to know which type of OTG ( Fig 3 of [1] ) you have in
> mind (given the current FRI ).

I do not have this publication with me, at this moment. So, I do not know
exactly what you are referring to. But I do have one generic remark: the
major feature of the LWR+FRI is its impedance control, and using this
potential to do traditional, purely geometric motion generation is a
waste, I think.

> Furthermore, I assume we do a time synchronized, kinematic time optimization.

What do you mean exactly?

> [1] @article{Kroeger:10,
> author = {T.~Kr{\"o}ger and F.~M.~Wahl},
> title = {On-Line Trajectory Generation: {B}asic Concepts for
> Instantaneous Reactions to Unforeseen Events},
> journal = {IEEE Trans. on Robotics},
> month = {feb},
> volume = {26},
> number = {1},
> pages = {94--111},
> year = {2010}
> }
>
> Can this problem can be formulated as an MPC (Model Predictive
> Control) problem? May be MPC is computationally too expensive in this
> case ?

MPC with only kinematic models is a bit overkill. (Or rather, people don't
attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
case, since it fits well to the dynamics approach that one chooses
(implicitly) when doing impedance control. But in this case, the concept of
kinematic trajectory generation looses its appeal (and sense), as I
mentioned before. In addition, the whole thing then becomes a complete
application ("sensing, planning, control") and not just a "trajectory
generation". Hence, real robotics! :-)

Probably it is better to move this discussion to orocos-dev, since the
matter becomes rather deeply technical, and there is nothing here that
users can profit from immediately.

Herman

gajan's picture

interruptible moveTo

As suggested by Herman I am moving this topic to orocos-dev from orocos-users

Task description: Add blended PTP functionally KUKA-LWR robot.
Control strategy of the robot: Cartesian Impedance Control (strategy 20)
\tau_Cmd =J^T(kc(x_FRI -x_msr)+F_FRI)+D(d_c)+f_dynamics(q,\dot q,\ddot q) --(1)
F_FRI: Additional superposed Cartesian Force
k_c: Cartesian Stiffness,
D(d_c), damping term.

Constrains:
q_max, \dot q_max, \ddot q_max, where q is the vector representing the
joint angles

>> Furthermore, I assume we do a time synchronized, kinematic time
>> optimization.
> What do you mean exactly?
time synchronized: All joints reach their target state at the same time instant
kinematic time optimization: Using only the kinematic equations.

> MPC with only kinematic models is a bit overkill. (Or rather, people don't
> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
> case, since it fits well to the dynamics approach that one chooses
> (implicitly) when doing impedance control. But in this case, the concept of
> kinematic trajectory generation looses its appeal (and sense), as I
> mentioned before. In addition, the whole thing then becomes a complete
> application ("sensing, planning, control") and not just a "trajectory
> generation". Hence, real robotics! :-)
I agree. But without a "good" knowledge of f_dynamics(q,\dot q,\ddot
q) in equation (1), is the dynamics approach feasible ? I would
appreciate if you can give us some directions to start with.

Gajan

P.S: Intuitively, using impedance control to achieve blended PTP makes
sense. But I guess that to obtain good results with cartesian
impedance control, the joint torque controller should overcome the
joint friction disturbance and gravity. I don't know how well this is
done in the LWR. Hopefully It is done well.

On Fri, Mar 11, 2011 at 8:18 PM, Herman Bruyninckx
<Herman [dot] Bruyninckx [..] ...> wrote:
> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
>
>> Hi Herman,
>>
>>> There is no such implementation currently provided. I personally made a
>>> matlab version of such a thing some years ago, but I have never published
>>> it because (i) the paper that I wrote about it was never accepted, (ii)
>>> it
>>> was not a closed-form solution and needed some numerical iterations, and
>>> (iii) it is not that useful really when you are doing sensor-based robot
>>> motions (as is our core activity in Leuven). For the latter, we use our
>>> constrained-based motion generation, ("ITaSC"), as published here
>>> @Article{ deschutter2006,
>>> author = {De~Schutter, Joris and De~Laet, Tinne and
>>> Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben
>>> and
>>> Aertbeli\"en, Erwin and Claes, Kasper and
>>> Bruyninckx, Herman},
>>> title = {Constraint-Based Task Specification and Estimation
>>> for Sensor-Based Robot Systems in the Presence of
>>> Geometric Uncertainty},
>>> journal = ijrr,
>>> volume = {26},
>>> number = {5},
>>> pages = {433--455},
>>> year = {2007}
>>> }
>>>
>>> and for which a 2.x implementation will be ready "soon".
>>
>> Looking forward to it.
>>
>>> I am interested in discussing the design of a "blended PTP" trajectory
>>> generator with you, on this mailinglist, in case you are motivated to
>>> work
>>> on such a thing.
>>> Torsten Kroeger
>>>
>>> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
>>> has the most complete version of such a thing, but its solution is rather
>>> complex. And again, overkill in sensor-based tasks.
>>>
>> I am definitely interested in implementing a blended PTP trajectory
>> generator.
>> I would like to know which type of OTG ( Fig 3 of [1] ) you have in
>> mind (given the current FRI ).
>
> I do not have this publication with me, at this moment. So, I do not know
> exactly what you are referring to. But I do have one generic remark: the
> major feature of the LWR+FRI is its impedance control, and using this
> potential to do traditional, purely geometric motion generation is a
> waste, I think.
>
>> Furthermore, I assume we do a time synchronized, kinematic time
>> optimization.
>
> What do you mean exactly?
>
>
>> [1] @article{Kroeger:10,
>> author = {T.~Kr{\"o}ger and F.~M.~Wahl},
>> title = {On-Line Trajectory Generation: {B}asic Concepts for
>> Instantaneous Reactions to Unforeseen Events},
>> journal = {IEEE Trans. on Robotics},
>> month = {feb},
>> volume = {26},
>> number = {1},
>> pages = {94--111},
>> year = {2010}
>> }
>>
>> Can this problem can be formulated as an MPC (Model Predictive
>> Control) problem? May be MPC is computationally too expensive in this
>> case ?
>
> MPC with only kinematic models is a bit overkill. (Or rather, people don't
> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
> case, since it fits well to the dynamics approach that one chooses
> (implicitly) when doing impedance control. But in this case, the concept of
> kinematic trajectory generation looses its appeal (and sense), as I
> mentioned before. In addition, the whole thing then becomes a complete
> application ("sensing, planning, control") and not just a "trajectory
> generation". Hence, real robotics! :-)
>
> Probably it is better to move this discussion to orocos-dev, since the
> matter becomes rather deeply technical, and there is nothing here that
> users can profit from immediately.
>
> Herman
>

interruptible moveTo

On Tue, 15 Mar 2011, Gajamohan Mohanarajah wrote:

> As suggested by Herman I am moving this topic to orocos-dev from orocos-users

Thanks!

> Task description: Add blended PTP functionally KUKA-LWR robot.
> Control strategy of the robot: Cartesian Impedance Control (strategy 20)
> \tau_Cmd =J^T(kc(x_FRI -x_msr)+F_FRI)+D(d_c)+f_dynamics(q,\dot q,\ddot q) --(1)
> F_FRI: Additional superposed Cartesian Force
> k_c: Cartesian Stiffness,
> D(d_c), damping term.
>
> Constrains:
> q_max, \dot q_max, \ddot q_max, where q is the vector representing the
> joint angles

There is something "strange" about using (only) geometric constraints on a
robot arm that has an dynamic controller. In any case, one can only satisfy
these constraints by "looking into the future" a little bit, because of the
arm and controller dynamics.

>>> Furthermore, I assume we do a time synchronized, kinematic time
>>> optimization.
>> What do you mean exactly?
> time synchronized: All joints reach their target state at the same time
> instant kinematic time optimization: Using only the kinematic equations.

Again, this does not fit well on an impedance controller.

>> MPC with only kinematic models is a bit overkill. (Or rather, people don't
>> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
>> case, since it fits well to the dynamics approach that one chooses
>> (implicitly) when doing impedance control. But in this case, the concept of
>> kinematic trajectory generation looses its appeal (and sense), as I
>> mentioned before. In addition, the whole thing then becomes a complete
>> application ("sensing, planning, control") and not just a "trajectory
>> generation". Hence, real robotics! :-)
> I agree. But without a "good" knowledge of f_dynamics(q,\dot q,\ddot
> q) in equation (1), is the dynamics approach feasible ?

No, not really.

> I would appreciate if you can give us some directions to start with.

I would start by listing the pros and cons of the approach and goals you
have in mind.
Pros:
- the kinematic constraints up to acceleration level are a natural
extension to what exists already;
- there is some literature available.
Cons:
- a dynamic controller with only geometric constraints is strange; I would
at least expect a maximum torque constraint, for example
- the perfect solution will be of the "MPC type"
- it might be much easier to let the controller make sure that the
constraints are not reached by a simple "potential field" approach that
keeps the joints away from their limits (in position, velocity and
acceleration).
- this is, of course, "only" an approximation, but the "perfect solution"
above also relies on the availability of a "perfectly known" dynamic
model, and that model is not really there.

> Gajan
>
> P.S: Intuitively, using impedance control to achieve blended PTP makes
> sense. But I guess that to obtain good results with cartesian
> impedance control, the joint torque controller should overcome the
> joint friction disturbance and gravity. I don't know how well this is
> done in the LWR. Hopefully It is done well.

It is done extremely well! In the sense that the resulting dynamic model is
much more in line with "perfect" linear dynamics than the real dynamics of
the robot :-)

Herman

> On Fri, Mar 11, 2011 at 8:18 PM, Herman Bruyninckx
> <Herman [dot] Bruyninckx [..] ...> wrote:
>> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
>>
>>> Hi Herman,
>>>
>>>> There is no such implementation currently provided. I personally made a
>>>> matlab version of such a thing some years ago, but I have never published
>>>> it because (i) the paper that I wrote about it was never accepted, (ii)
>>>> it
>>>> was not a closed-form solution and needed some numerical iterations, and
>>>> (iii) it is not that useful really when you are doing sensor-based robot
>>>> motions (as is our core activity in Leuven). For the latter, we use our
>>>> constrained-based motion generation, ("ITaSC"), as published here
>>>> @Article{ deschutter2006,
>>>> author = {De~Schutter, Joris and De~Laet, Tinne and
>>>> Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben
>>>> and
>>>> Aertbeli\"en, Erwin and Claes, Kasper and
>>>> Bruyninckx, Herman},
>>>> title = {Constraint-Based Task Specification and Estimation
>>>> for Sensor-Based Robot Systems in the Presence of
>>>> Geometric Uncertainty},
>>>> journal = ijrr,
>>>> volume = {26},
>>>> number = {5},
>>>> pages = {433--455},
>>>> year = {2007}
>>>> }
>>>>
>>>> and for which a 2.x implementation will be ready "soon".
>>>
>>> Looking forward to it.
>>>
>>>> I am interested in discussing the design of a "blended PTP" trajectory
>>>> generator with you, on this mailinglist, in case you are motivated to
>>>> work
>>>> on such a thing.
>>>> Torsten Kroeger
>>>>
>>>> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
>>>> has the most complete version of such a thing, but its solution is rather
>>>> complex. And again, overkill in sensor-based tasks.
>>>>
>>> I am definitely interested in implementing a blended PTP trajectory
>>> generator.
>>> I would like to know which type of OTG ( Fig 3 of [1] ) you have in
>>> mind (given the current FRI ).
>>
>> I do not have this publication with me, at this moment. So, I do not know
>> exactly what you are referring to. But I do have one generic remark: the
>> major feature of the LWR+FRI is its impedance control, and using this
>> potential to do traditional, purely geometric motion generation is a
>> waste, I think.
>>
>>> Furthermore, I assume we do a time synchronized, kinematic time
>>> optimization.
>>
>> What do you mean exactly?
>>
>>
>>> [1] @article{Kroeger:10,
>>> author = {T.~Kr{\"o}ger and F.~M.~Wahl},
>>> title = {On-Line Trajectory Generation: {B}asic Concepts for
>>> Instantaneous Reactions to Unforeseen Events},
>>> journal = {IEEE Trans. on Robotics},
>>> month = {feb},
>>> volume = {26},
>>> number = {1},
>>> pages = {94--111},
>>> year = {2010}
>>> }
>>>
>>> Can this problem can be formulated as an MPC (Model Predictive
>>> Control) problem? May be MPC is computationally too expensive in this
>>> case ?
>>
>> MPC with only kinematic models is a bit overkill. (Or rather, people don't
>> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
>> case, since it fits well to the dynamics approach that one chooses
>> (implicitly) when doing impedance control. But in this case, the concept of
>> kinematic trajectory generation looses its appeal (and sense), as I
>> mentioned before. In addition, the whole thing then becomes a complete
>> application ("sensing, planning, control") and not just a "trajectory
>> generation". Hence, real robotics! :-)
>>
>> Probably it is better to move this discussion to orocos-dev, since the
>> matter becomes rather deeply technical, and there is nothing here that
>> users can profit from immediately.
>>
>> Herman

gajan's picture

interruptible moveTo

Hi,

As a first step towards an interruptible moveTo command for KUKA LWR,
We implemented a time optimal trajectory generator which can handle non-zero
initial velocities under joint velocity and acceleration constraints.

You can get the code at
https://github.com/IDSCETHZurich/trajectory-generator

TrajectoryGenerator
-------------------
* This component implements two event ports which listen to commanded
* Cartesian positions and/or joint angles of a robot.
* Based on the commands and the current state (position, VELOCITY) of the
* robot a time optimal velocity profile is created, using the
* VelocityProfile_NonZeroInit class, and the joint positions are sequentially
* sent out on an outputPort.
* The provided working example makes use of the kinematic solver given by
* in the KUKALWR_Kinematics class. However, by selecting an appropriate
* kinematics Solver, the component can be used to control any robot.

VelocityProfile_NonZeroInit
---------------------------
* This class extends the KDL::VelocityProfile abstract class,
generating velocity
* profiles which are time optimal (subjected to maximum accelerations and
* velocities) and may have non-zero initial velocities.
* It calculates the polynomials of piece-wise linear velocity profiles and
* the corresponding position and acceleration profiles.
* Then it provides functions to obtain position/velocity/acceleration
* of the trajectory at a certain time instant.

KUKALWR_Kinematics
------------------
* This class defines the kinematic transformations on the KUKA LWR Robot
*
* The Forward Kinematics transformation considers the whole 7 DOF of the
* robot, while the Inverse Kinematics only calculates 6 Joints, setting
* the extra DOF to zero. This allows us to obtain a closed-form solution
* for the inverse kinematics.
*
* The equations of this closed-form solution are based on the IK of an
* anthropomorphic arm with a spherical wrist given in the book
* "Robotics - Modelling, Planning and Control", by Siciliano et al. (2010)
* Only one of the four possible solutions for the kinematic
* chain is calculated. Specifically, the right-shoulder elbow-up one.

It would be great if we can get some feedback on the component and
some comments on
future directions to pursue.

Best Regards,
Gajan

On Tue, Mar 15, 2011 at 10:37 AM, Herman Bruyninckx
<Herman [dot] Bruyninckx [..] ...> wrote:
> On Tue, 15 Mar 2011, Gajamohan Mohanarajah wrote:
>
>> As suggested by Herman I am moving this topic to orocos-dev from
>> orocos-users
>
> Thanks!
>
>> Task description: Add blended PTP functionally KUKA-LWR robot.
>> Control strategy of the robot: Cartesian Impedance Control (strategy 20)
>> \tau_Cmd =J^T(kc(x_FRI -x_msr)+F_FRI)+D(d_c)+f_dynamics(q,\dot q,\ddot q)
>> --(1)
>> F_FRI: Additional superposed Cartesian Force
>> k_c: Cartesian Stiffness,
>> D(d_c), damping term.
>>
>> Constrains:
>> q_max, \dot q_max, \ddot q_max, where q is the vector representing the
>> joint angles
>
> There is something "strange" about using (only) geometric constraints on a
> robot arm that has an dynamic controller. In any case, one can only satisfy
> these constraints by "looking into the future" a little bit, because of the
> arm and controller dynamics.
>
>>>> Furthermore, I assume we do a time synchronized, kinematic time
>>>> optimization.
>>>
>>> What do you mean exactly?
>>
>> time synchronized: All joints reach their target state at the same time
>> instant kinematic time optimization: Using only the kinematic equations.
>
> Again, this does not fit well on an impedance controller.
>
>>> MPC with only kinematic models is a bit overkill. (Or rather, people
>>> don't
>>> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
>>> case, since it fits well to the dynamics approach that one chooses
>>> (implicitly) when doing impedance control. But in this case, the concept
>>> of
>>> kinematic trajectory generation looses its appeal (and sense), as I
>>> mentioned before. In addition, the whole thing then becomes a complete
>>> application ("sensing, planning, control") and not just a "trajectory
>>> generation". Hence, real robotics! :-)
>>
>> I agree. But without a "good" knowledge of f_dynamics(q,\dot q,\ddot
>> q) in equation (1), is the dynamics approach feasible ?
>
> No, not really.
>
>> I would appreciate if you can give us some directions to start with.
>
> I would start by listing the pros and cons of the approach and goals you
> have in mind. Pros:
> - the kinematic constraints up to acceleration level are a natural
>  extension to what exists already; - there is some literature available.
> Cons:
> - a dynamic controller with only geometric constraints is strange; I would
>  at least expect a maximum torque constraint, for example
> - the perfect solution will be of the "MPC type"
> - it might be much easier to let the controller make sure that the
>  constraints are not reached by a simple "potential field" approach that
>  keeps the joints away from their limits (in position, velocity and
>  acceleration). - this is, of course, "only" an approximation, but the
> "perfect solution"
>  above also relies on the availability of a "perfectly known" dynamic
>  model, and that model is not really there.
>
>> Gajan
>>
>> P.S: Intuitively, using impedance control to achieve blended PTP makes
>> sense. But I guess that to obtain good results with cartesian
>> impedance control, the joint torque controller should overcome the
>> joint friction disturbance and gravity. I don't know how well this is
>> done in the LWR. Hopefully It is done well.
>
> It is done extremely well! In the sense that the resulting dynamic model is
> much more in line with "perfect" linear dynamics than the real dynamics of
> the robot :-)
>
> Herman
>
>> On Fri, Mar 11, 2011 at 8:18 PM, Herman Bruyninckx
>> <Herman [dot] Bruyninckx [..] ...> wrote:
>>>
>>> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
>>>
>>>> Hi Herman,
>>>>
>>>>> There is no such implementation currently provided. I personally made a
>>>>> matlab version of such a thing some years ago, but I have never
>>>>> published
>>>>> it because (i) the paper that I wrote about it was never accepted, (ii)
>>>>> it
>>>>> was not a closed-form solution and needed some numerical iterations,
>>>>> and
>>>>> (iii) it is not that useful really when you are doing sensor-based
>>>>> robot
>>>>> motions (as is our core activity in Leuven). For the latter, we use our
>>>>> constrained-based motion generation, ("ITaSC"), as published here
>>>>> @Article{    deschutter2006,
>>>>>  author          = {De~Schutter, Joris and De~Laet, Tinne and
>>>>>                    Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben
>>>>> and
>>>>>                    Aertbeli\"en, Erwin and Claes, Kasper and
>>>>>                    Bruyninckx, Herman},
>>>>>  title           = {Constraint-Based Task Specification and Estimation
>>>>>                    for Sensor-Based Robot Systems in the Presence of
>>>>> Geometric Uncertainty},
>>>>>  journal         = ijrr,
>>>>>  volume          = {26},
>>>>>  number          = {5},
>>>>>  pages           = {433--455},
>>>>>  year            = {2007}
>>>>> }
>>>>>
>>>>> and for which a 2.x implementation will be ready "soon".
>>>>
>>>> Looking forward to it.
>>>>
>>>>> I am interested in discussing the design of a "blended PTP" trajectory
>>>>> generator with you, on this mailinglist, in case you are motivated to
>>>>> work
>>>>> on such a thing.
>>>>> Torsten Kroeger
>>>>>
>>>>>
>>>>> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
>>>>> has the most complete version of such a thing, but its solution is
>>>>> rather
>>>>> complex. And again, overkill in sensor-based tasks.
>>>>>
>>>> I am definitely interested in implementing a blended PTP trajectory
>>>> generator.
>>>> I would like to know which type of OTG ( Fig 3 of [1] ) you have in
>>>> mind (given the current FRI ).
>>>
>>> I do not have this publication with me, at this moment. So, I do not know
>>> exactly what you are referring to. But I do have one generic remark: the
>>> major feature of the LWR+FRI is its impedance control, and using this
>>> potential to do traditional, purely geometric motion generation is a
>>> waste, I think.
>>>
>>>> Furthermore, I assume we do a time synchronized, kinematic time
>>>> optimization.
>>>
>>> What do you mean exactly?
>>>
>>>
>>>> [1] @article{Kroeger:10,
>>>>       author = {T.~Kr{\"o}ger and F.~M.~Wahl},
>>>>       title = {On-Line Trajectory Generation: {B}asic Concepts for
>>>> Instantaneous Reactions to Unforeseen Events},
>>>>       journal = {IEEE Trans. on Robotics},
>>>>       month = {feb},
>>>>       volume = {26},
>>>>       number = {1},
>>>>       pages = {94--111},
>>>>       year = {2010}
>>>> }
>>>>
>>>> Can this problem can be formulated as an MPC (Model Predictive
>>>> Control) problem? May be MPC is computationally too expensive in this
>>>> case ?
>>>
>>> MPC with only kinematic models is a bit overkill. (Or rather, people
>>> don't
>>> attach this name to it.) MPC becomes extremely relevant for the LWR+FRI
>>> case, since it fits well to the dynamics approach that one chooses
>>> (implicitly) when doing impedance control. But in this case, the concept
>>> of
>>> kinematic trajectory generation looses its appeal (and sense), as I
>>> mentioned before. In addition, the whole thing then becomes a complete
>>> application ("sensing, planning, control") and not just a "trajectory
>>> generation". Hence, real robotics! :-)
>>>
>>> Probably it is better to move this discussion to orocos-dev, since the
>>> matter becomes rather deeply technical, and there is nothing here that
>>> users can profit from immediately.
>>>
>>> Herman
>

interruptible moveTo

On Mon, 16 May 2011, Gajamohan Mohanarajah wrote:

> As a first step towards an interruptible moveTo command for KUKA LWR,
> We implemented a time optimal trajectory generator which can handle non-zero
> initial velocities under joint velocity and acceleration constraints.
>
> You can get the code at
> https://github.com/IDSCETHZurich/trajectory-generator

Thanks!

In what sense exactly is this a "first step towards an interruptible moveTo command"?

> TrajectoryGenerator
> -------------------
> * This component implements two event ports which listen to commanded
> * Cartesian positions and/or joint angles of a robot.
> * Based on the commands and the current state (position, VELOCITY) of the
> * robot a time optimal velocity profile is created, using the
> * VelocityProfile_NonZeroInit class, and the joint positions are sequentially
> * sent out on an outputPort.
> * The provided working example makes use of the kinematic solver given by
> * in the KUKALWR_Kinematics class. However, by selecting an appropriate
> * kinematics Solver, the component can be used to control any robot.
>
> VelocityProfile_NonZeroInit
> ---------------------------
> * This class extends the KDL::VelocityProfile abstract class,
> generating velocity
> * profiles which are time optimal (subjected to maximum accelerations and
> * velocities) and may have non-zero initial velocities.

Are the _final_ velocities zero? (I guess so...)

> * It calculates the polynomials of piece-wise linear velocity profiles and
> * the corresponding position and acceleration profiles.

Do you have a document available somewhere that explains the algorithm?

> * Then it provides functions to obtain position/velocity/acceleration
> * of the trajectory at a certain time instant.
>
> KUKALWR_Kinematics
> ------------------
> * This class defines the kinematic transformations on the KUKA LWR Robot
> *
> * The Forward Kinematics transformation considers the whole 7 DOF of the
> * robot, while the Inverse Kinematics only calculates 6 Joints, setting
> * the extra DOF to zero. This allows us to obtain a closed-form solution
> * for the inverse kinematics.

That seems to be a very restrictive view on using redundant manipulators...
KDL has already IK solvers with full use of all available DOFs.

> * The equations of this closed-form solution are based on the IK of an
> * anthropomorphic arm with a spherical wrist given in the book
> * "Robotics - Modelling, Planning and Control", by Siciliano et al. (2010)
> * Only one of the four possible solutions for the kinematic
> * chain is calculated. Specifically, the right-shoulder elbow-up one.
>
> It would be great if we can get some feedback on the component and
> some comments on future directions to pursue.

I think it would be better to fully decouple the trajectory generation from
the specific robot that you use it for. I mean: I do not expect to see
anything about the LWR in a subdirectory like
"trajectory-generator/trajectory_generator/src"

Some files do not have a license, e.g., "trajectory-generator /
trajectory_generator / src / VelocityProfile_NonZeroInit.cpp".

That same file has string-based IO in its code; it would be better to
separate the computations (including the configuration computations) from
the IO.

I see some "magic numbers" in some files (e.g., "0.0001" in the
above-mentioned file), which should be replaced by variables, configurable
via a Configuration interface.

> Best Regards,
> Gajan

Herman

interruptible moveTo

On Fri, 20 May 2011, Gajamohan Mohanarajah wrote:

> Hi,
>
> Thank you for the valuable comments.
>
> On Mon, May 16, 2011 at 7:58 PM, Herman Bruyninckx
> <Herman [dot] Bruyninckx [..] ...> wrote:
>> On Mon, 16 May 2011, Gajamohan Mohanarajah wrote:
>>
>>> As a first step towards an interruptible moveTo command for KUKA LWR,
>>> We implemented a time optimal trajectory generator which can handle
>>> non-zero
>>> initial velocities under joint velocity and acceleration constraints.
>>>
>>> You can get the code at
>>> https://github.com/IDSCETHZurich/trajectory-generator
>>
>> Thanks!
>>
>> In what sense exactly is this a "first step towards an interruptible moveTo
>> command"?
> We think it is a first step since the following additions can be thought of:
> 1. Synchronize all joints, this will save joints experiencing full
> acceleration/deceleration all the time.
> 2. Consider Jerk.
> 3. Non-zero final velocity (?)

Thanks for your documentation. I now fully understand what you want to do.
And that is a rather trivial extension of the code that is already in
Orocos for quite some years (and which works with zero initial velocities).
Which is fine, but then it would maybe be better to extend/patch that
original code (and make it configurable to work with non-zero initial
velocities), instead of introducing a completely new code.

Some interesting literature on these things:

<http://people.mech.kuleuven.be/~bruyninc/blender/doc/interpolation-api.html>

@InProceedings{ meyerdirkx94,
author = {Meyer, Piotr J. and Dirkx, Walrick A. A. F.},
title = {A Time-Optimal Cubic Setpoint Generator with On-Fly
Parameter Modification},
booktitle = {Proc. 3rd {P}hilips Conf. Applications of Systems and
Control Theory},
year = {1994},
address = {Doorwerth, The Netherlands},
keywords = {kinematics, motion planning}
}

@Article{ RewKim2010,
author = {Rew, Keun-Ho and Kim, Kyung-Soo},
title = {A Closed-Form Solution to Asymmetric Motion
Profile Allowing Acceleration Manipulation},
journal = {IEEE Transactions on Industrial Electronics},
year = {2010}
}

Herman

>>> TrajectoryGenerator
>>> -------------------
>>> *   This component implements two event ports which listen to commanded
>>> *   Cartesian positions and/or joint angles of a robot.
>>> *   Based on the commands and the current state (position, VELOCITY) of
>>> the
>>> *   robot a time optimal velocity profile is created, using the
>>> *   VelocityProfile_NonZeroInit class, and the joint positions are
>>> sequentially
>>> *   sent out on an outputPort.
>>> *   The provided working example makes use of the kinematic solver given
>>> by
>>> *   in the KUKALWR_Kinematics class. However, by selecting an appropriate
>>> *   kinematics Solver, the component can be used to control any robot.
>>>
>>> VelocityProfile_NonZeroInit
>>> ---------------------------
>>> *   This class extends the KDL::VelocityProfile abstract class,
>>> generating velocity
>>> *   profiles which are time optimal (subjected to maximum accelerations
>>> and
>>> *   velocities) and may have non-zero initial velocities.
>>
>> Are the _final_ velocities zero? (I guess so...)
> Yes, The final velocities are zero.
>
>>
>>> *   It calculates the polynomials of piece-wise linear velocity profiles
>>> and
>>> *   the corresponding position and acceleration profiles.
>>
>> Do you have a document available somewhere that explains the algorithm?
> I am attaching a document compiled by my colleague Ramos Francisco.
>
>>> *   Then it provides functions to obtain position/velocity/acceleration
>>> *   of the trajectory at a certain time instant.
>>>
>>> KUKALWR_Kinematics
>>> ------------------
>>> *   This class defines the kinematic transformations on the KUKA LWR Robot
>>> *
>>> *   The Forward Kinematics transformation considers the whole 7 DOF of the
>>> *   robot, while the Inverse Kinematics only calculates 6 Joints, setting
>>> *   the extra DOF to zero. This allows us to obtain a closed-form solution
>>> *   for the inverse kinematics.
>>
>> That seems to be a very restrictive view on using redundant manipulators...
>> KDL has already IK solvers with full use of all available DOFs.
>
> We moved all the KUKA related kinematics into a different component [kuka_IK].
> trajectory-generators components now has a single input (event) port
> for joint angles.
>
>>> *   The equations of this closed-form solution are based on the IK of an
>>> *   anthropomorphic arm with a spherical wrist given in the book
>>> *   "Robotics - Modelling, Planning and Control", by Siciliano et al.
>>> (2010)
>>> *   Only one of the four possible solutions for the kinematic
>>> *   chain is calculated. Specifically, the right-shoulder elbow-up one.
>>>
>>> It would be great if we can get some feedback on the component and
>>> some comments on future directions to pursue.
>>
>> I think it would be better to fully decouple the trajectory generation from
>> the specific robot that you use it for. I mean: I do not expect to see
>> anything about the LWR in a subdirectory like
>> "trajectory-generator/trajectory_generator/src"
> Fixed.
>
>> Some files do not have a license, e.g., "trajectory-generator /
>> trajectory_generator / src / VelocityProfile_NonZeroInit.cpp".
>> That same file has string-based IO in its code; it would be better to
>> separate the computations (including the configuration computations) from
>> the IO.
> Fixing. Will commit soon.
>
>> I see some "magic numbers" in some files (e.g., "0.0001" in the
>> above-mentioned file), which should be replaced by variables, configurable
>> via a Configuration interface.
>
>>> Best Regards,
>>> Gajan
>>
>> Herman
>>
>
> Gajan
>

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

interruptible moveTo

On Mar 11, 2011, at 00:32 , Herman Bruyninckx wrote:

> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
>
>> We currently use the moveTo command of OCL::CartesianGeneratorPos
>> (1.1) to move our lwr.
>>
>> 1. Now, between the commanded cartesian positions the robot does a
>> stop and go motion. Is there any components available that does a
>> continuous movement between the commanded points ? something like the
>> continuous PTP of KRL. (we basically need an interruptible moveTo
>> command.)
>
> There is no such implementation currently provided. I personally made a
> matlab version of such a thing some years ago, but I have never published
> it because (i) the paper that I wrote about it was never accepted, (ii) it
> was not a closed-form solution and needed some numerical iterations, and
> (iii) it is not that useful really when you are doing sensor-based robot
> motions (as is our core activity in Leuven). For the latter, we use our
> constrained-based motion generation, ("ITaSC"), as published here
> @Article{ deschutter2006,
> author = {De~Schutter, Joris and De~Laet, Tinne and
> Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben and
> Aertbeli\"en, Erwin and Claes, Kasper and
> Bruyninckx, Herman},
> title = {Constraint-Based Task Specification and Estimation
> for Sensor-Based Robot Systems in the Presence of
> Geometric Uncertainty},
> journal = ijrr,
> volume = {26},
> number = {5},
> pages = {433--455},
> year = {2007}
> }
>
> and for which a 2.x implementation will be ready "soon".
>
>
> I am interested in discussing the design of a "blended PTP" trajectory
> generator with you, on this mailinglist, in case you are motivated to work
> on such a thing.
> Torsten Kroeger
> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
> has the most complete version of such a thing, but its solution is rather
> complex. And again, overkill in sensor-based tasks.
>
>> [In http://svn.mech.kuleuven.be/repos/orocos/trunk/kul-ros-pkg/pr2_lwr_coupl...
>> the author writes the arriving ROS TF frame directly to the
>> FRIComponent. This was not possible to us unless the commanded and the
>> measured postions were close. Otherwise the robot hits velocity
>> limits.
>
> This is, in my opinion, the number one problem with the current FRI: it
> does not warn you a bit in advance that it is going to hit its limits :-(
>
>> Therefore we need an interpolator in between the friComponent
>> and our custom component that gives out desired Cartesian positions]
>>
>> 2. Where can we find the OCL::CartesianGeneratorPos for 2.0 ?

It doesn't exist. It was considered unmaintaned in v1 and summarily dropped.
S

Ruben Smits's picture

interruptible moveTo

On 11 Mar 2011, at 13:51, S Roderick wrote:

> On Mar 11, 2011, at 00:32 , Herman Bruyninckx wrote:
>
>> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
>>
>>> We currently use the moveTo command of OCL::CartesianGeneratorPos
>>> (1.1) to move our lwr.
>>>
>>> 1. Now, between the commanded cartesian positions the robot does a
>>> stop and go motion. Is there any components available that does a
>>> continuous movement between the commanded points ? something like the
>>> continuous PTP of KRL. (we basically need an interruptible moveTo
>>> command.)
>>
>> There is no such implementation currently provided. I personally made a
>> matlab version of such a thing some years ago, but I have never published
>> it because (i) the paper that I wrote about it was never accepted, (ii) it
>> was not a closed-form solution and needed some numerical iterations, and
>> (iii) it is not that useful really when you are doing sensor-based robot
>> motions (as is our core activity in Leuven). For the latter, we use our
>> constrained-based motion generation, ("ITaSC"), as published here
>> @Article{ deschutter2006,
>> author = {De~Schutter, Joris and De~Laet, Tinne and
>> Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben and
>> Aertbeli\"en, Erwin and Claes, Kasper and
>> Bruyninckx, Herman},
>> title = {Constraint-Based Task Specification and Estimation
>> for Sensor-Based Robot Systems in the Presence of
>> Geometric Uncertainty},
>> journal = ijrr,
>> volume = {26},
>> number = {5},
>> pages = {433--455},
>> year = {2007}
>> }
>>
>> and for which a 2.x implementation will be ready "soon".
>>
>>
>> I am interested in discussing the design of a "blended PTP" trajectory
>> generator with you, on this mailinglist, in case you are motivated to work
>> on such a thing.
>> Torsten Kroeger
>> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generation>
>> has the most complete version of such a thing, but its solution is rather
>> complex. And again, overkill in sensor-based tasks.
>>
>>> [In http://svn.mech.kuleuven.be/repos/orocos/trunk/kul-ros-pkg/pr2_lwr_coupl...
>>> the author writes the arriving ROS TF frame directly to the
>>> FRIComponent. This was not possible to us unless the commanded and the
>>> measured postions were close. Otherwise the robot hits velocity
>>> limits.
>>
>> This is, in my opinion, the number one problem with the current FRI: it
>> does not warn you a bit in advance that it is going to hit its limits :-(
>>
>>> Therefore we need an interpolator in between the friComponent
>>> and our custom component that gives out desired Cartesian positions]
>>>
>>> 2. Where can we find the OCL::CartesianGeneratorPos for 2.0 ?
>
> It doesn't exist. It was considered unmaintaned in v1 and summarily dropped.

But we secretly ported one to v2.x ;):

git clone http://people.mech.kuleuven.be/~rsmits/Public_Git/trajectory_generators.git

It hasn't been build/tested for a while now but you can get the idea out of this one.

> S

-- Ruben

>
>

interruptible moveTo

On Friday 11 March 2011 14:06:04 Ruben Smits wrote:
> On 11 Mar 2011, at 13:51, S Roderick wrote:
> > On Mar 11, 2011, at 00:32 , Herman Bruyninckx wrote:
> >> On Fri, 11 Mar 2011, Gajamohan Mohanarajah wrote:
> >>> We currently use the moveTo command of OCL::CartesianGeneratorPos
> >>> (1.1) to move our lwr.
> >>>
> >>> 1. Now, between the commanded cartesian positions the robot does a
> >>> stop and go motion. Is there any components available that does a
> >>> continuous movement between the commanded points ? something like the
> >>> continuous PTP of KRL. (we basically need an interruptible moveTo
> >>> command.)
> >>
> >> There is no such implementation currently provided. I personally made a
> >> matlab version of such a thing some years ago, but I have never
> >> published it because (i) the paper that I wrote about it was never
> >> accepted, (ii) it was not a closed-form solution and needed some
> >> numerical iterations, and (iii) it is not that useful really when you
> >> are doing sensor-based robot motions (as is our core activity in
> >> Leuven). For the latter, we use our constrained-based motion
> >> generation, ("ITaSC"), as published here @Article{ deschutter2006,
> >>
> >> author = {De~Schutter, Joris and De~Laet, Tinne and
> >>
> >> Rutgeerts, Johan and Decr\'e, Wilm and Smits, Ruben
> >> and Aertbeli\"en, Erwin and Claes, Kasper and
> >> Bruyninckx, Herman},
> >>
> >> title = {Constraint-Based Task Specification and Estimation
> >>
> >> for Sensor-Based Robot Systems in the Presence of
> >>
> >> Geometric Uncertainty},
> >>
> >> journal = ijrr,
> >> volume = {26},
> >> number = {5},
> >> pages = {433--455},
> >> year = {2007}
> >>
> >> }
> >>
> >> and for which a 2.x implementation will be ready "soon".
> >>
> >>
> >> I am interested in discussing the design of a "blended PTP" trajectory
> >> generator with you, on this mailinglist, in case you are motivated to
> >> work on such a thing.
> >> Torsten Kroeger
> >> <http://cs.stanford.edu/people/tkr/research/on-line-trajectory-generatio
> >> n> has the most complete version of such a thing, but its solution is
> >> rather complex. And again, overkill in sensor-based tasks.
> >>
> >>> [In
> >>> http://svn.mech.kuleuven.be/repos/orocos/trunk/kul-ros-pkg/pr2_lwr_cou
> >>> pling/src/Connector_single.cpp the author writes the arriving ROS TF
> >>> frame directly to the
> >>> FRIComponent. This was not possible to us unless the commanded and the
> >>> measured postions were close. Otherwise the robot hits velocity
> >>> limits.
> >>
> >> This is, in my opinion, the number one problem with the current FRI: it
> >> does not warn you a bit in advance that it is going to hit its limits
> >> :-(
> >>
> >>> Therefore we need an interpolator in between the friComponent
> >>> and our custom component that gives out desired Cartesian positions]
> >>>
> >>> 2. Where can we find the OCL::CartesianGeneratorPos for 2.0 ?
> >
> > It doesn't exist. It was considered unmaintaned in v1 and summarily
> > dropped.
>
> But we secretly ported one to v2.x ;):
>
> git clone
> http://people.mech.kuleuven.be/~rsmits/Public_Git/trajectory_generators.gi
> t
>
> It hasn't been build/tested for a while now but you can get the idea out of
> this one.

Could we use these components for the YouBot demo ?

Peter