Open yy16808 opened 1 year ago
Try specifying openravepy.IkFilterOptions.IgnoreEndEffectorCollisions|openravepy.IkFilterOptions.IgnoreEndEffectorEnvCollisions|openravepy.IkFilterOptions.IgnoreEndEffectorSelfCollisions|openravepy.IkFilterOptions.IgnoreJointLimits|openravepy.IkFilterOptions.IgnoreSelfCollisions
to filteroptions, then remove the flag one by one
Try specifying
openravepy.IkFilterOptions.IgnoreEndEffectorCollisions|openravepy.IkFilterOptions.IgnoreEndEffectorEnvCollisions|openravepy.IkFilterOptions.IgnoreEndEffectorSelfCollisions|openravepy.IkFilterOptions.IgnoreJointLimits|openravepy.IkFilterOptions.IgnoreSelfCollisions
to filteroptions, then remove the flag one by one
Thanks, I think you got to the heart of the problem!
But I have a little question:
I just got the CPP solver file through the command "python
openrave-config --python-dir/openravepy/_openravepy_/ikfast.py --robot=*.dae --iktype=transform6d --baselink=0 --eelink=6 --savefile=$(pwd)/ikfast61.cpp
" and then tested the file alone in C++ environment.
I check the ikfast.py and find that it seems don't support the filteroptions.
Before that how could I configure the filteroptions?
It depends on how you call the solver but I can show these filteroptions occurrences at least
include/openrave/robot.h
298: /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution.
299: bool FindIKSolution(const IkParameterization& param, std::vector<dReal>& solution, int filteroptions) const;
300: bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filteroptions) const;
301: bool FindIKSolution(const IkParameterization& param, int filteroptions, IkReturnPtr ikreturn) const;
302: bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn) const;
308: /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution.
309: bool FindIKSolutions(const IkParameterization& param, std::vector<std::vector<dReal> >& solutions, int filteroptions) const;
310: bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& solutions, int filteroptions) const;
311: bool FindIKSolutions(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const;
312: bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const;
It depends on how you call the solver but I can show these filteroptions occurrences at least
include/openrave/robot.h 298: /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution. 299: bool FindIKSolution(const IkParameterization& param, std::vector<dReal>& solution, int filteroptions) const; 300: bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filteroptions) const; 301: bool FindIKSolution(const IkParameterization& param, int filteroptions, IkReturnPtr ikreturn) const; 302: bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn) const; 308: /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution. 309: bool FindIKSolutions(const IkParameterization& param, std::vector<std::vector<dReal> >& solutions, int filteroptions) const; 310: bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& solutions, int filteroptions) const; 311: bool FindIKSolutions(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const; 312: bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const;
As far as I know, these functions can be used in the concrete example like that
robot.SetDOFValues([2.58, 0.547, 1.5, -0.7],[0,1,2,3]) # set the first 4 dof values
Tee = manip.GetTransform() # get end effector
ikparam = IkParameterization(Tee[0:3,3],ikmodel.iktype) # build up the translation3d ik query
sols = manip.FindIKSolutions(ikparam, IkFilterOptions.CheckEnvCollisions) # get all solutions
But I still could not find the interface to modify the demo like that to configure the IkFilterOptions:
.. code-block:: python
env = Environment()
kinbody = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(kinbody)
solver = ikfast.IKFastSolver(kinbody=kinbody)
chaintree = solver.generateIkSolver(baselink=0,eelink=7,freeindices=[2],solvefn=ikfast.IKFastSolver.solveFullIK_6D)
code = solver.writeIkSolver(chaintree)
with open('ik.cpp','w') as f:
f.write(code)
.. _ikfast_generatedcpp:
sols = manip.FindIKSolutions(ikparam, IkFilterOptions.CheckEnvCollisions) # get all solutions
change IkFilterOptions.CheckEnvCollisions
to IkFilterOptions.IgnoreEndEffectorCollisions|IkFilterOptions.IgnoreEndEffectorEnvCollisions|IkFilterOptions.IgnoreEndEffectorSelfCollisions|IkFilterOptions.IgnoreJointLimits|IkFilterOptions.IgnoreSelfCollisions
sols = manip.FindIKSolutions(ikparam, IkFilterOptions.CheckEnvCollisions) # get all solutions
change
IkFilterOptions.CheckEnvCollisions
toIkFilterOptions.IgnoreEndEffectorCollisions|IkFilterOptions.IgnoreEndEffectorEnvCollisions|IkFilterOptions.IgnoreEndEffectorSelfCollisions|IkFilterOptions.IgnoreJointLimits|IkFilterOptions.IgnoreSelfCollisions
Yes, I know your meaning, But It is still a concrete example with specifying for ikparam
I mean the sencond demo to generate a whole CPP file with collision options without a specific positional parameter .
iksolver itself is unrelated to filteroptions and is capable to give all solutions. filteroptions is an argument for FindIKSolutions.
iksolver itself is unrelated to filteroptions and is capable to give all solutions. filteroptions is an argument for FindIKSolutions.
The picture above I have no set with filteroptions, just the iksolver output.
But I don't think it gives the all solutions...
I tested the IKfast for my own robot, But sometimes it seems could not give the full solutions: sometimes output 4 or 2 solutions, less than 8 as below
The solutions piture:
The robot demo: