Re: [OpenRAVE-users] Replacement for GetFullTrajectoryFromActive

[Dmitry B. <ber...@gm...>]

Good question! We're integrating some planners I have written in openrave
with ROS and another system called ACH. This requires us to convert
openrave trajectories to other formats. We need to have the full trajectory
b/c I only plan for a subset of robot dofs for any given motion, but the
other systems require a full robot state trajectory.

Dmitry


On Fri, Dec 21, 2012 at 12:30 PM, Rosen Diankov <ros...@gm...>wrote:

> glad you got it working!
>
> just curious, what is your use case for converting to the full trajectory?
>
> 2012/12/22 Dmitry Berenson <ber...@gm...>:
> > Hi Rosen,
> >
> > Yes, that makes sense about not storing pointers. You might consider
> storing
> > the names of the bodies, though, in an easy-to-access way in the future.
> >
> > Anyway, I've put everything together and it works! For others' reference
> the
> > code to convert active trajectories to full trajectories (even when there
> > are multiple moving bodies in a single traj) is:
> >
> >     ConfigurationSpecification activespec =
> > ptraj->GetConfigurationSpecification();
> >     std::set<KinBodyPtr> sbodies;
> >     FOREACH(itgroup, activespec._vgroups) {
> >       stringstream ss(itgroup->name);
> >       string type, bodyname;
> >       ss >> type;
> >       ss >> bodyname;
> >       //RAVELOG_INFO("Bodyname: %s\n",bodyname.c_str());
> >       if(GetEnv()->GetKinBody(bodyname))
> >       {
> >         sbodies.insert(GetEnv()->GetKinBody(bodyname));
> >       }
> >     }
> >
> >     ConfigurationSpecification spec;
> >     set <KinBodyPtr>::iterator si;
> >     for (si=sbodies.begin(); si!=sbodies.end(); si++)
> >     {
> >         std::vector<int> indices((*si)->GetDOF());
> >         for(int i = 0; i < (*si)->GetDOF(); ++i) {
> >             indices[i] = i;
> >         }
> >         ConfigurationSpecification tempspec =
> > (*si)->GetConfigurationSpecificationIndices(indices, "linear");
> >         tempspec.AddDerivativeGroups(1,true); // add velocity + timestamp
> >         spec += tempspec;
> >     }
> >
> >     OpenRAVE::planningutils::ConvertTrajectorySpecification(ptraj, spec);
> >
> >
> > Thanks again Rosen!
> >
> > Dmitry
> >
> >
> > On Fri, Dec 21, 2012 at 11:56 AM, Rosen Diankov <ros...@gm...
> >
> > wrote:
> >>
> >> Unfortunately ConfiguraitonSpecification was designed to be
> >> serializable into XML and initialized solely through XML,  and it is
> >> not dependent on any environment, hence it cannot contain any pointers
> >> to bodies. We would have to write a function separate from
> >> ConfigurationSpecification to return this information, most likely in
> >> the planningutils namespace.
> >>
> >> The function would look something like:
> >>
> >> std::set<KinBodyPtr> bodies;
> >> FOREACH(itgroup, spec._vgroups) {
> >>   stringstream ss(itgroup->name);
> >>   string type, bodyname;
> >>   ss >> type >> bodyname;
> >>   bodies.insert(env->GetKinBody(bodyname));
> >> }
> >>
> >> now "bodies" contains all the unique bodies of the specification.
> >>
> >>
> >> > I worry that this will be quite sensitive to the definition of the
> name
> >> > strings in the group and if this changes in the future, the code will
> break
> >>
> >> Usually robot names or important object names do not change, however
> >> pointers to objects are much more sensitive and are only valid within
> >> one environment instance.
> >>
> >> In any case, you can find the default supported group strings and
> >> their formats here:
> >>
> >>
> >>
> http://openrave.org/docs/latest_stable/coreapihtml/classOpenRAVE_1_1ConfigurationSpecification_1_1Group.html
> >>
> >> (of course, you can also come up with your own custom groups)
> >>
> >>
> >> rosen,
> >>
> >> 2012/12/22 Dmitry Berenson <ber...@gm...>:
> >> > Hi Rosen,
> >> >
> >> > I took a look at the "name" field in the groups:
> >> >
> >> >  int numgroups =
> ptraj->GetConfigurationSpecification()._vgroups.size();
> >> >     RAVELOG_INFO("num groups in traj: %d\n",numgroups);
> >> >     for(int i = 0; i < numgroups; i++)
> >> >     {
> >> >         RAVELOG_INFO("name %d:
> >> >
> >> >
> %s\n",i,ptraj->GetConfigurationSpecification()._vgroups[i].name.c_str());
> >> >     } but it seems like
> >> >
> >> > For instance, for door opening I get:
> >> >
> >> > [cbirrtproblem.cpp:536] num groups in traj: 5
> >> > [cbirrtproblem.cpp:539] name 0: deltatime
> >> > [cbirrtproblem.cpp:539] name 1: joint_velocities BarrettWAM 0 1 2 3 4
> 5
> >> > 6
> >> > [cbirrtproblem.cpp:539] name 2: joint_velocities kitchen 6
> >> > [cbirrtproblem.cpp:539] name 3: joint_values BarrettWAM 0 1 2 3 4 5 6
> >> > [cbirrtproblem.cpp:539] name 4: joint_values kitchen 6
> >> >
> >> >
> >> >
> >> > It seems somewhat cumbersome to parse these group name strings and
> >> > extract
> >> > the name of the robot (especially since the robot name is repeated in
> >> > several groups), which will require checking for duplicates. I worry
> >> > that
> >> > this will be quite sensitive to the definition of the name strings in
> >> > the
> >> > group and if this changes in the future, the code will break. Is there
> >> > any
> >> > other way to get the names of the robots involved in a trajectory? It
> >> > would
> >> > be nice if the ConfigurationSpec had an element which was just a
> vector
> >> > of
> >> > pointers to the involved kinbodies.
> >> >
> >> >
> >> > Dmitry
> >> >
> >> > On Fri, Dec 21, 2012 at 11:22 AM, Rosen Diankov
> >> > <ros...@gm...>
> >> > wrote:
> >> >>
> >> >> oops, i forgot the "robot->"
> >> >>
> >> >> The configuration specification groups hold the name of the bodies,
> so
> >> >> converting and adding configurations can be done without a problem.
> >> >> you can use the += or + operators. To convert the trajectory, just
> >> >> create a ConfigurationSpecification that += all the full specs of
> each
> >> >> of the bodies, then call
> >> >> ConvertTrajectorySpecification once.
> >> >>
> >> >> We also have two C++ examples for planning with multiple bodies at
> the
> >> >> same time:
> >> >>
> >> >>
> >> >>
> >> >>
> http://openrave.org/docs/latest_stable/coreapihtml/orplanning_multirobot_8cpp-example.html
> >> >>
> >> >> this one is more involved since we need to sample valid
> configurations
> >> >> of the robot and the door at the same time:
> >> >>
> >> >>
> >> >>
> >> >>
> http://openrave.org/docs/latest_stable/coreapihtml/orplanning_door_8cpp-example.html
> >> >>
> >> >> rosen,
> >> >>
> >> >> 2012/12/22 Dmitry Berenson <ber...@gm...>:
> >> >> > Ah, I see the problem, the "GetConfigurationSpecificationIndices"
> is
> >> >> > a
> >> >> > member function of KinBody, it needed to have a "robot->" in front
> of
> >> >> > it.
> >> >> >
> >> >> > Anyway, this works! Thank you!
> >> >> >
> >> >> > One last question: for trajectories that involve more than one
> >> >> > kinbody
> >> >> > (for
> >> >> > instance opening a door with an arm), how would I go about
> converting
> >> >> > the
> >> >> > active configuration spec to a full spec for all bodies? For
> >> >> > instance,
> >> >> > is
> >> >> > there a way to see which kinbodies are being used in a given
> >> >> > trajectory,
> >> >> > then do the above process for each and add up the specs?
> >> >> >
> >> >> >
> >> >> >
> >> >> > Dmitry
> >> >> >
> >> >> >
> >> >> > On Fri, Dec 21, 2012 at 11:04 AM, Rosen Diankov
> >> >> > <ros...@gm...>
> >> >> > wrote:
> >> >> >>
> >> >> >> it's been in openrave since 0.6.
> >> >> >>
> >> >> >> here's 0.8.0 documentation:
> >> >> >>
> >> >> >>
> >> >> >>
> >> >> >>
> >> >> >>
> http://openrave.org/docs/0.8.0/coreapihtml/classOpenRAVE_1_1KinBody.html#ac49a826522e27a650211cf6f18046c5d
> >> >> >>
> >> >> >> 2012/12/22 Dmitry Berenson <ber...@gm...>:
> >> >> >> > Hi Rosen,
> >> >> >> >
> >> >> >> > It seems I don't have the function
> >> >> >> > GetConfigurationSpecificationIndices
> >> >> >> > in
> >> >> >> > my version of openrave. I'm somewhat stuck with using openrave
> 0.8
> >> >> >> > b/c
> >> >> >> > that's what my collaborators are using. Would it be possible to
> do
> >> >> >> > this
> >> >> >> > without using that function?
> >> >> >> >
> >> >> >> > Dmitry
> >> >> >> >
> >> >> >> >
> >> >> >> > On Fri, Dec 21, 2012 at 10:40 AM, Rosen Diankov
> >> >> >> > <ros...@gm...>
> >> >> >> > wrote:
> >> >> >> >>
> >> >> >> >> ok i think that's a bug. most likely it's coming from the fact
> >> >> >> >> that
> >> >> >> >> the specification now contains the robot transform
> (quaternion),
> >> >> >> >> and
> >> >> >> >> probably that is getting initialized with all 0s, and
> obviously a
> >> >> >> >> quaternion with all 0s is invalid.
> >> >> >> >>
> >> >> >> >> can you try the following instead:
> >> >> >> >>
> >> >> >> >> std::vector<int> indices(robot->GetDOF());
> >> >> >> >> for(int i = 0; i < robot->GetDOF(); ++i) {
> >> >> >> >>     indices[i] = i;
> >> >> >> >> }
> >> >> >> >> ConfigurationSpecification spec =
> >> >> >> >> GetConfigurationSpecificationIndices(indices, "parabolic");
> >> >> >> >> spec.AddDerivativeGroups(1, true); // add velocity + timestamp
> >> >> >> >>
> >> >> >> >> rosen,
> >> >> >> >>
> >> >> >> >>
> >> >> >> >> 2012/12/22 Dmitry Berenson <ber...@gm...>:
> >> >> >> >> > Hi Rosen,
> >> >> >> >> >
> >> >> >> >> > Hmm, now it's giving me a different problem when I try to
> >> >> >> >> > execute
> >> >> >> >> > it:
> >> >> >> >> >
> >> >> >> >> > [environment-core.h:2108] simulation thread exception:
> openrave
> >> >> >> >> > (Assert):
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> >
> [/home/berenson/openrave_planning/openrave/openrave_svn/include/openrave/geometry.h:240]
> >> >> >> >> > -> OpenRAVE::geometry::RaveVector<T>&
> >> >> >> >> > OpenRAVE::geometry::RaveVector<T>::normalize4() [with T =
> >> >> >> >> > double],
> >> >> >> >> > expr:
> >> >> >> >> > f >
> >> >> >> >> > 0
> >> >> >> >> >
> >> >> >> >> > It prints this out over and over again when I play the
> >> >> >> >> > trajectory
> >> >> >> >> > and
> >> >> >> >> > the
> >> >> >> >> > robot doesn't move.
> >> >> >> >> >
> >> >> >> >> > I'm using this code:
> >> >> >> >> >
> >> >> >> >> >     ConfigurationSpecification spec =
> >> >> >> >> > robot->GetConfigurationSpecification();
> >> >> >> >> >     spec.AddDerivativeGroups(1,true); // add velocity +
> >> >> >> >> > timestamp
> >> >> >> >> >
> >> >> >> >> >
> OpenRAVE::planningutils::ConvertTrajectorySpecification(ptraj,
> >> >> >> >> > spec);
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> > Dmitry
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> > On Thu, Dec 20, 2012 at 9:20 PM, Rosen Diankov
> >> >> >> >> > <ros...@gm...>
> >> >> >> >> > wrote:
> >> >> >> >> >>
> >> >> >> >> >> hi dmitry,
> >> >> >> >> >>
> >> >> >> >> >> i see where the problem is, the
> >> >> >> >> >> robot->GetConfigurationSpecification()
> >> >> >> >> >> is just the joint values + transform of the robot, and does
> >> >> >> >> >> not
> >> >> >> >> >> contain timing and velocity information. you might want to
> do
> >> >> >> >> >> this:
> >> >> >> >> >>
> >> >> >> >> >> ConfigurationSpecification spec =
> >> >> >> >> >> robot->GetConfigurationSpecification();
> >> >> >> >> >> spec.AddDerivativeGroups(1,true); // add velocity +
> timestamp
> >> >> >> >> >>
> >> >> >> >> >> rosen,
> >> >> >> >> >>
> >> >> >> >> >> 2012/12/21 Dmitry Berenson <ber...@gm...>:
> >> >> >> >> >> > Hi Rosen,
> >> >> >> >> >> >
> >> >> >> >> >> > I've changed the trajectory to the full spec and wrote it
> >> >> >> >> >> > out,
> >> >> >> >> >> > but
> >> >> >> >> >> > it
> >> >> >> >> >> > seems
> >> >> >> >> >> > I'm unable to execute it. Here is the convert:
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >     ConfigurationSpecification spec =
> >> >> >> >> >> > robot->GetConfigurationSpecification();
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> OpenRAVE::planningutils::ConvertTrajectorySpecification(ptraj,
> >> >> >> >> >> > spec);
> >> >> >> >> >> >
> >> >> >> >> >> > When I try to execute it, I get this error:
> >> >> >> >> >> >
> >> >> >> >> >> > openravepy._openravepy_.openravepy_ext.openrave_exception:
> >> >> >> >> >> > openrave
> >> >> >> >> >> > (Assert):
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> [/home/berenson/openrave_planning/openrave/openrave_svn/src/libopenrave-core/generictrajectory.cpp:271]
> >> >> >> >> >> > -> virtual void
> >> >> >> >> >> > OpenRAVE::GenericTrajectory::Sample(std::vector<double>&,
> >> >> >> >> >> > OpenRAVE::dReal, const
> >> >> >> >> >> > OpenRAVE::ConfigurationSpecification&)
> >> >> >> >> >> > const,
> >> >> >> >> >> > expr:
> >> >> >> >> >> > _timeoffset>=0
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> > Should I have to re-time the trajectory? If so, how can I
> do
> >> >> >> >> >> > this
> >> >> >> >> >> > easily?
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> > Dmitry
> >> >> >> >> >> >
> >> >> >> >> >> >
> >> >> >> >> >> > On Wed, Dec 19, 2012 at 5:43 PM, Dmitry Berenson
> >> >> >> >> >> > <ber...@gm...>
> >> >> >> >> >> > wrote:
> >> >> >> >> >> >>
> >> >> >> >> >> >> Thanks Rosen! I'll try this.
> >> >> >> >> >> >>
> >> >> >> >> >> >> Dmitry
> >> >> >> >> >> >>
> >> >> >> >> >> >>
> >> >> >> >> >> >> On Tue, Dec 18, 2012 at 7:38 PM, Rosen Diankov
> >> >> >> >> >> >> <ros...@gm...>
> >> >> >> >> >> >> wrote:
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> hi dmitry,
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> truthfully, it doesn't really matter anymore. you can
> pass
> >> >> >> >> >> >>> in
> >> >> >> >> >> >>> any
> >> >> >> >> >> >>> type
> >> >> >> >> >> >>> of trajectory for the robot controller. if you really
> want
> >> >> >> >> >> >>> to
> >> >> >> >> >> >>> "full
> >> >> >> >> >> >>> trajectory", then you would have to convert to a new
> >> >> >> >> >> >>> specification.
> >> >> >> >> >> >>> Fortunately, there is a function
> >> >> >> >> >> >>> planningutils::ConvertTrajectorySpecification that will
> do
> >> >> >> >> >> >>> exactly
> >> >> >> >> >> >>> that:
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> http://openrave.org/docs/latest_stable/coreapihtml/namespaceOpenRAVE_1_1planningutils.html#ab9a70727a771437bd118dd4d66e895ac
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> you can get the full robot specification that includes
> >> >> >> >> >> >>> robot
> >> >> >> >> >> >>> position
> >> >> >> >> >> >>> from
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> robot->GetConfigurationSpecification()
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> hope this helps,
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> rosen,
> >> >> >> >> >> >>>
> >> >> >> >> >> >>>
> >> >> >> >> >> >>> 2012/12/19 Dmitry Berenson <ber...@gm...>:
> >> >> >> >> >> >>> > Hi Rosen,
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> > I used to use "GetFullTrajectoryFromActive" to get a
> >> >> >> >> >> >>> > trajectory
> >> >> >> >> >> >>> > for
> >> >> >> >> >> >>> > all
> >> >> >> >> >> >>> > the
> >> >> >> >> >> >>> > DOF of the robot from an active trajectory. I see that
> >> >> >> >> >> >>> > this
> >> >> >> >> >> >>> > is
> >> >> >> >> >> >>> > deprecated
> >> >> >> >> >> >>> > now.
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> > Is there an easy way to do this using the new
> >> >> >> >> >> >>> > configuration
> >> >> >> >> >> >>> > specifications?
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> > Thanks!
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> > Dmitry
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> ------------------------------------------------------------------------------
> >> >> >> >> >> >>> > LogMeIn Rescue: Anywhere, Anytime Remote support for
> IT.
> >> >> >> >> >> >>> > Free
> >> >> >> >> >> >>> > Trial
> >> >> >> >> >> >>> > Remotely access PCs and mobile devices and provide
> >> >> >> >> >> >>> > instant
> >> >> >> >> >> >>> > support
> >> >> >> >> >> >>> > Improve your efficiency, and focus on delivering more
> >> >> >> >> >> >>> > value-add
> >> >> >> >> >> >>> > services
> >> >> >> >> >> >>> > Discover what IT Professionals Know. Rescue delivers
> >> >> >> >> >> >>> > http://p.sf.net/sfu/logmein_12329d2d
> >> >> >> >> >> >>> > _______________________________________________
> >> >> >> >> >> >>> > Openrave-users mailing list
> >> >> >> >> >> >>> > Ope...@li...
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>> >
> https://lists.sourceforge.net/lists/listinfo/openrave-users
> >> >> >> >> >> >>> >
> >> >> >> >> >> >>
> >> >> >> >> >> >>
> >> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >> >
> >> >> >> >
> >> >> >> >
> >> >> >
> >> >> >
> >> >
> >> >
> >
> >
>