## ompl-users

 [ompl-users] OMPL and real robot arm From: antoine aigueperse - 2013-07-08 16:26:12 Attachments: Message as HTML ```Hello OMPL users,I’m a beginner in OMPL, but this library looks very good and quite simple to use it (I hope for me!!). I have a question, maybe it is a stupid question and already solved in previous post. If it is the case sorry, I didn’t saw the post. I want to use OMPL to estimate path for an articulated robot arm having 7 DoF. I define the space which is composed to “RealVectorStateSpace(numjoints)” and joints bounds (see code below). Now I’m trying to compute distance between to ScopedState. My problem is that the distance cannot take into a count the joints bounds. For example the distance of the joint defined such as “armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0);” gives => space->distance(129 , 231) = 102. Normally it should returned: abs(231-360) + abs(129) (distance respecting joint bounds). I think that, I will have the same problem for the interpolation. Is it possible to define a metric function (like si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, _1)); )? A solution is to create a complete state space using RealVectorStateSpace implementation? Thank in advance Antoine ________________________________ #include #include #include #include bool isStateValid(const ompl::base::State *state) { const ompl::base::RealVectorStateSpace::StateType *s = state->as(); double X = s->values[0]; double Y = s->values[1]; if (X >=-170 && X <=-50 && Y >=0 && Y <=-30) return false; else return true; } void test(void) { /// construct the state space we are planning in int numjoints = 7; ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(numjoints)); ///We then set the bounds for the R^{numjoints} component of this state space: ompl::base::RealVectorBounds armbounds(numjoints); armbounds.setLow(0, -185.0); armbounds.setHigh(0, 0.0); armbounds.setLow(1, -155.0); armbounds.setHigh(1, 35.0); armbounds.setLow(2, 0.0); armbounds.setHigh(2,154.0); armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0); armbounds.setLow(4, -130.0); armbounds.setHigh(4,130.0); armbounds.setLow(5, -180.0); armbounds.setHigh(5,180.0); armbounds.setLow(6, -180.0); armbounds.setHigh(6,180.0); space->as()->setBounds(armbounds); space->setup(); // not necessary!! ///Create an instance of ompl::base::SpaceInformation for the state space ompl::base::SpaceInformationPtr si(new ompl::base::SpaceInformation(space)); ///Set the state validity checker si->setStateValidityChecker(boost::bind(&isStateValid, _1)); si->setup(); ///Create a random start state: ompl::base::ScopedState<> start(space); start[0] = -0 ; start[1] = 0 ; start[2] = 0 ; start[3] = 0 ; start[4] = 129 ; start[5] = 0 ; start[6] = 0; std::cout<< "start.print() = " < goal(space); goal[0] = 0 ; goal[1] = 0 ; goal[2] = 0 ; goal[3] = 0 ; goal[4] = 231 ; goal[5] = 0 ; goal[6] = 0; std::cout<< "goal.print() = " <
 Re: [ompl-users] OMPL and real robot arm From: Ioan Sucan - 2013-07-08 16:42:22 Attachments: Message as HTML ```Hello Antoine, When you are calling functions from the state space, it is expected that states are within bounds, except for calls to enforceBounds() and satisfiesBounds(). So, calling the distance function with the value 231, when max is 230, is considered invalid. For efficiency reasons, there is no check, but behaviour is not defined. I am not sure what you mean with 360 - 231 to be considered instead. Do you mean that enforceBounds() should convert 231 to 360-231 (when max is 230)? Or do you have revolute joints? If so, perhaps using SO2StateSpace for some joints would help? To change the distance function you have to inherit from the RealVectorStateSpace and provide your own implementation of distance(). It sounds like that may be the case, if you would like distance() to behave in a specific way. You would most likely need to implement interpolate() as well. I hope this helps. Please let me know if I did not understand your question correctly. Ioan On Mon, Jul 8, 2013 at 7:26 PM, antoine aigueperse wrote: > Hello OMPL users,I’m a beginner in OMPL, but this library looks very good > and quite simple to use it (I hope for me!!). > > I have a question, maybe it is a stupid question and already solved in > previous post. If it is the case sorry, I didn’t saw the post. > I want to use OMPL to estimate path for an articulated robot arm having 7 > DoF. I define the space which is composed to > “RealVectorStateSpace(numjoints)” and joints bounds (see code below). Now > I’m trying to compute distance between to ScopedState. My problem is that > the distance cannot take into a count the joints bounds. > > For example the distance of the joint defined such as “armbounds.setLow(3, > -130.0); armbounds.setHigh(3,230.0);” gives => space->distance(129 , 231) > = 102. Normally it should returned: abs(231-360) + abs(129) (distance > respecting joint bounds). I think that, I will have the same problem for > the interpolation. > > Is it possible to define a metric function (like > si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, > _1)); )? A solution is to create a complete state space using > RealVectorStateSpace implementation? > > Thank in advance > > Antoine > > ________________________________ > > #include > > #include > #include > #include > > > bool isStateValid(const ompl::base::State *state) > { > const ompl::base::RealVectorStateSpace::StateType *s = > state->as(); > > double X = s->values[0]; > double Y = s->values[1]; > > if (X >=-170 && X <=-50 && Y >=0 && Y <=-30) > return false; > > else > return true; > > } > > void test(void) > { > /// construct the state space we are planning in > int numjoints = 7; > ompl::base::StateSpacePtr space(new > ompl::base::RealVectorStateSpace(numjoints)); > > > ///We then set the bounds for the R^{numjoints} component of this > state space: > ompl::base::RealVectorBounds armbounds(numjoints); > > armbounds.setLow(0, -185.0); armbounds.setHigh(0, 0.0); > armbounds.setLow(1, -155.0); armbounds.setHigh(1, 35.0); > armbounds.setLow(2, 0.0); armbounds.setHigh(2,154.0); > armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0); > armbounds.setLow(4, -130.0); armbounds.setHigh(4,130.0); > armbounds.setLow(5, -180.0); armbounds.setHigh(5,180.0); > armbounds.setLow(6, -180.0); armbounds.setHigh(6,180.0); > > space->as()->setBounds(armbounds); > space->setup(); // not necessary!! > > ///Create an instance of ompl::base::SpaceInformation for the state > space > ompl::base::SpaceInformationPtr si(new > ompl::base::SpaceInformation(space)); > > ///Set the state validity checker > si->setStateValidityChecker(boost::bind(&isStateValid, _1)); > si->setup(); > > ///Create a random start state: > ompl::base::ScopedState<> start(space); > start[0] = -0 ; start[1] = 0 ; start[2] = 0 ; start[3] = 0 ; start[4] > = 129 ; start[5] = 0 ; start[6] = 0; > std::cout<< "start.print() = " < start.print(std::cout); > > ///And a random goal state: > ompl::base::ScopedState<> goal(space); > goal[0] = 0 ; goal[1] = 0 ; goal[2] = 0 ; goal[3] = 0 ; goal[4] = 231 > ; goal[5] = 0 ; goal[6] = 0; > std::cout<< "goal.print() = " < goal.print(std::cout); > > // Check range axis > std::cout<< "goal.satisfiesBounds = " << > space->satisfiesBounds(goal.get()) < > // Compute L2 distance > std::cout<< "goal.distance = " << > space->distance(start.get(),goal.get()) < > std::cout<< "goal.enforcebounds = "; > space->enforceBounds(goal.get()); > goal.print(std::cout); > space->printSettings(std::cout); > > } > > int main() > { > test(); > } > > > ------------------------------------------------------------------------------ > This SF.net email is sponsored by Windows: > > Build for Windows Store. > > http://p.sf.net/sfu/windows-dev2dev > _______________________________________________ > ompl-users mailing list > ompl-users@... > https://lists.sourceforge.net/lists/listinfo/ompl-users > > ```
 Re: [ompl-users] OMPL and real robot arm From: antoine aigueperse - 2013-07-11 07:57:21 Attachments: Message as HTML ```Hello Ioan, Sorry, I forget to said the most important: I'm working only with revolute joints. To take into a count the bounds in functions distance() and interpolate(), i create a new state space based on the RealVectorStateSpace. If you want i can transfer you the class (or commit on your SVN)? Thanks, antoine 2013/7/8 Ioan Sucan > Hello Antoine, > > When you are calling functions from the state space, it is expected that > states are within bounds, except for calls to enforceBounds() and > satisfiesBounds(). > So, calling the distance function with the value 231, when max is 230, is > considered invalid. For efficiency reasons, there is no check, but > behaviour is not defined. > I am not sure what you mean with 360 - 231 to be considered instead. Do > you mean that enforceBounds() should convert 231 to 360-231 (when max is > 230)? Or do you have revolute joints? If so, perhaps using SO2StateSpace > for some joints would help? > > To change the distance function you have to inherit from the > RealVectorStateSpace and provide your own implementation of distance(). It > sounds like that may be the case, if you would like distance() to behave in > a specific way. You would most likely need to implement interpolate() as > well. > I hope this helps. Please let me know if I did not understand your > question correctly. > > Ioan > > > > On Mon, Jul 8, 2013 at 7:26 PM, antoine aigueperse > wrote: > >> Hello OMPL users,I’m a beginner in OMPL, but this library looks very good >> and quite simple to use it (I hope for me!!). >> >> I have a question, maybe it is a stupid question and already solved in >> previous post. If it is the case sorry, I didn’t saw the post. >> I want to use OMPL to estimate path for an articulated robot arm having 7 >> DoF. I define the space which is composed to >> “RealVectorStateSpace(numjoints)” and joints bounds (see code below). Now >> I’m trying to compute distance between to ScopedState. My problem is that >> the distance cannot take into a count the joints bounds. >> >> For example the distance of the joint defined such as >> “armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0);” gives => >> space->distance(129 , 231) = 102. Normally it should returned: abs(231-360) >> + abs(129) (distance respecting joint bounds). I think that, I will have >> the same problem for the interpolation. >> >> Is it possible to define a metric function (like >> si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, >> _1)); )? A solution is to create a complete state space using >> RealVectorStateSpace implementation? >> >> Thank in advance >> >> Antoine >> >> ________________________________ >> >> #include >> >> #include >> #include >> #include >> >> >> bool isStateValid(const ompl::base::State *state) >> { >> const ompl::base::RealVectorStateSpace::StateType *s = >> state->as(); >> >> double X = s->values[0]; >> double Y = s->values[1]; >> >> if (X >=-170 && X <=-50 && Y >=0 && Y <=-30) >> return false; >> >> else >> return true; >> >> } >> >> void test(void) >> { >> /// construct the state space we are planning in >> int numjoints = 7; >> ompl::base::StateSpacePtr space(new >> ompl::base::RealVectorStateSpace(numjoints)); >> >> >> ///We then set the bounds for the R^{numjoints} component of this >> state space: >> ompl::base::RealVectorBounds armbounds(numjoints); >> >> armbounds.setLow(0, -185.0); armbounds.setHigh(0, 0.0); >> armbounds.setLow(1, -155.0); armbounds.setHigh(1, 35.0); >> armbounds.setLow(2, 0.0); armbounds.setHigh(2,154.0); >> armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0); >> armbounds.setLow(4, -130.0); armbounds.setHigh(4,130.0); >> armbounds.setLow(5, -180.0); armbounds.setHigh(5,180.0); >> armbounds.setLow(6, -180.0); armbounds.setHigh(6,180.0); >> >> space->as()->setBounds(armbounds); >> space->setup(); // not necessary!! >> >> ///Create an instance of ompl::base::SpaceInformation for the state >> space >> ompl::base::SpaceInformationPtr si(new >> ompl::base::SpaceInformation(space)); >> >> ///Set the state validity checker >> si->setStateValidityChecker(boost::bind(&isStateValid, _1)); >> si->setup(); >> >> ///Create a random start state: >> ompl::base::ScopedState<> start(space); >> start[0] = -0 ; start[1] = 0 ; start[2] = 0 ; start[3] = 0 ; start[4] >> = 129 ; start[5] = 0 ; start[6] = 0; >> std::cout<< "start.print() = " <> start.print(std::cout); >> >> ///And a random goal state: >> ompl::base::ScopedState<> goal(space); >> goal[0] = 0 ; goal[1] = 0 ; goal[2] = 0 ; goal[3] = 0 ; goal[4] = 231 >> ; goal[5] = 0 ; goal[6] = 0; >> std::cout<< "goal.print() = " <> goal.print(std::cout); >> >> // Check range axis >> std::cout<< "goal.satisfiesBounds = " << >> space->satisfiesBounds(goal.get()) <> >> // Compute L2 distance >> std::cout<< "goal.distance = " << >> space->distance(start.get(),goal.get()) <> >> std::cout<< "goal.enforcebounds = "; >> space->enforceBounds(goal.get()); >> goal.print(std::cout); >> space->printSettings(std::cout); >> >> } >> >> int main() >> { >> test(); >> } >> >> >> ------------------------------------------------------------------------------ >> This SF.net email is sponsored by Windows: >> >> Build for Windows Store. >> >> http://p.sf.net/sfu/windows-dev2dev >> _______________________________________________ >> ompl-users mailing list >> ompl-users@... >> https://lists.sourceforge.net/lists/listinfo/ompl-users >> >> > ```
 Re: [ompl-users] OMPL and real robot arm From: Ioan Sucan - 2013-07-11 11:43:17 Attachments: Message as HTML ```Hello Antoine, Thank you! The best thing is to fork the ompl repository and send us a link / pull request to the changes. I think looking at the code will help me better understand what the issue with distance / interpolate is. Let us know if we can be of assistance! Ioan On Thu, Jul 11, 2013 at 10:57 AM, antoine aigueperse wrote: > Hello Ioan, > > Sorry, I forget to said the most important: I'm working only with revolute > joints. > > To take into a count the bounds in functions distance() and interpolate(), > i create a new state space based on the RealVectorStateSpace. If you want i > can transfer you the class (or commit on your SVN)? > > Thanks, > > antoine > > > 2013/7/8 Ioan Sucan > >> Hello Antoine, >> >> When you are calling functions from the state space, it is expected that >> states are within bounds, except for calls to enforceBounds() and >> satisfiesBounds(). >> So, calling the distance function with the value 231, when max is 230, is >> considered invalid. For efficiency reasons, there is no check, but >> behaviour is not defined. >> I am not sure what you mean with 360 - 231 to be considered instead. Do >> you mean that enforceBounds() should convert 231 to 360-231 (when max is >> 230)? Or do you have revolute joints? If so, perhaps using SO2StateSpace >> for some joints would help? >> >> To change the distance function you have to inherit from the >> RealVectorStateSpace and provide your own implementation of distance(). It >> sounds like that may be the case, if you would like distance() to behave in >> a specific way. You would most likely need to implement interpolate() as >> well. >> I hope this helps. Please let me know if I did not understand your >> question correctly. >> >> Ioan >> >> >> >> On Mon, Jul 8, 2013 at 7:26 PM, antoine aigueperse < >> a.aigueperse@...> wrote: >> >>> Hello OMPL users,I’m a beginner in OMPL, but this library looks very >>> good and quite simple to use it (I hope for me!!). >>> >>> I have a question, maybe it is a stupid question and already solved in >>> previous post. If it is the case sorry, I didn’t saw the post. >>> I want to use OMPL to estimate path for an articulated robot arm having >>> 7 DoF. I define the space which is composed to >>> “RealVectorStateSpace(numjoints)” and joints bounds (see code below). Now >>> I’m trying to compute distance between to ScopedState. My problem is that >>> the distance cannot take into a count the joints bounds. >>> >>> For example the distance of the joint defined such as >>> “armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0);” gives => >>> space->distance(129 , 231) = 102. Normally it should returned: abs(231-360) >>> + abs(129) (distance respecting joint bounds). I think that, I will have >>> the same problem for the interpolation. >>> >>> Is it possible to define a metric function (like >>> si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, >>> _1)); )? A solution is to create a complete state space using >>> RealVectorStateSpace implementation? >>> >>> Thank in advance >>> >>> Antoine >>> >>> ________________________________ >>> >>> #include >>> >>> #include >>> #include >>> #include >>> >>> >>> bool isStateValid(const ompl::base::State *state) >>> { >>> const ompl::base::RealVectorStateSpace::StateType *s = >>> state->as(); >>> >>> double X = s->values[0]; >>> double Y = s->values[1]; >>> >>> if (X >=-170 && X <=-50 && Y >=0 && Y <=-30) >>> return false; >>> >>> else >>> return true; >>> >>> } >>> >>> void test(void) >>> { >>> /// construct the state space we are planning in >>> int numjoints = 7; >>> ompl::base::StateSpacePtr space(new >>> ompl::base::RealVectorStateSpace(numjoints)); >>> >>> >>> ///We then set the bounds for the R^{numjoints} component of this >>> state space: >>> ompl::base::RealVectorBounds armbounds(numjoints); >>> >>> armbounds.setLow(0, -185.0); armbounds.setHigh(0, 0.0); >>> armbounds.setLow(1, -155.0); armbounds.setHigh(1, 35.0); >>> armbounds.setLow(2, 0.0); armbounds.setHigh(2,154.0); >>> armbounds.setLow(3, -130.0); armbounds.setHigh(3,230.0); >>> armbounds.setLow(4, -130.0); armbounds.setHigh(4,130.0); >>> armbounds.setLow(5, -180.0); armbounds.setHigh(5,180.0); >>> armbounds.setLow(6, -180.0); armbounds.setHigh(6,180.0); >>> >>> space->as()->setBounds(armbounds); >>> space->setup(); // not necessary!! >>> >>> ///Create an instance of ompl::base::SpaceInformation for the state >>> space >>> ompl::base::SpaceInformationPtr si(new >>> ompl::base::SpaceInformation(space)); >>> >>> ///Set the state validity checker >>> si->setStateValidityChecker(boost::bind(&isStateValid, _1)); >>> si->setup(); >>> >>> ///Create a random start state: >>> ompl::base::ScopedState<> start(space); >>> start[0] = -0 ; start[1] = 0 ; start[2] = 0 ; start[3] = 0 ; >>> start[4] = 129 ; start[5] = 0 ; start[6] = 0; >>> std::cout<< "start.print() = " <>> start.print(std::cout); >>> >>> ///And a random goal state: >>> ompl::base::ScopedState<> goal(space); >>> goal[0] = 0 ; goal[1] = 0 ; goal[2] = 0 ; goal[3] = 0 ; goal[4] = >>> 231 ; goal[5] = 0 ; goal[6] = 0; >>> std::cout<< "goal.print() = " <>> goal.print(std::cout); >>> >>> // Check range axis >>> std::cout<< "goal.satisfiesBounds = " << >>> space->satisfiesBounds(goal.get()) <>> >>> // Compute L2 distance >>> std::cout<< "goal.distance = " << >>> space->distance(start.get(),goal.get()) <>> >>> std::cout<< "goal.enforcebounds = "; >>> space->enforceBounds(goal.get()); >>> goal.print(std::cout); >>> space->printSettings(std::cout); >>> >>> } >>> >>> int main() >>> { >>> test(); >>> } >>> >>> >>> ------------------------------------------------------------------------------ >>> This SF.net email is sponsored by Windows: >>> >>> Build for Windows Store. >>> >>> http://p.sf.net/sfu/windows-dev2dev >>> _______________________________________________ >>> ompl-users mailing list >>> ompl-users@... >>> https://lists.sourceforge.net/lists/listinfo/ompl-users >>> >>> >> > ```