search

krishauser / Klampt

Kris' Locomotion and Manipulation Planning Toolkit
BSD 3-Clause "New" or "Revised" License
378 stars 96 forks source link

Nan value of commandedPosition on initialization #129

Closed ShaoxiongYao closed 2 years ago

ShaoxiongYao commented 2 years ago

Testing on klampt 0.9 branch. I used klampt_control.py with command, klampt_control common/kinova/complete_interface.py data/robots/kinova_gen3_7dof.urdf, where complete_interface.py has make function to create the RobotInterface. In line 37 of klampt_control.py: q = self.controller.configToKlampt(self.controller.commandedPosition()) the commandedPosition returns [nan, nan,...] at the initial configuration. And this caused problem in robotinterface.py, line 756, return model.configFromDrivers(config). I wonder if we should check whether there is at least one command sent to the robot driver. Or, this problem can be fixed by adding appropriate commandedPosition function to KinovaGen3RobotInterface.

krishauser commented 2 years ago

Can you try with the latest pull?

Certainly it would be helpful to have commandedPosition return something, but hypothetically the emulator should pull this from sensedPosition. I haven't tested this emulator functionality yet.

ShaoxiongYao commented 2 years ago

Tested on latest pull, not fixed, the NaN return value from commandedPosition still cause problem, the error message is

File "/usr/local/lib/python3.8/dist-packages/Klampt-0.9.0-py3.8-linux-x86_64.egg/klampt/apps/klampt_control.py", line 875, in main g_plugin = ControllerGLPlugin(world,controller,None) File "/usr/local/lib/python3.8/dist-packages/Klampt-0.9.0-py3.8-linux-x86_64.egg/klampt/apps/klampt_control.py", line 67, in __init__ qklampt = self.controller.configToKlampt(q) File "/usr/local/lib/python3.8/dist-packages/Klampt-0.9.0-py3.8-linux-x86_64.egg/klampt/control/robotinterfaceutils.py", line 2939, in configToKlampt return self._base.configToKlampt(config,klamptConfig,part=None,joint_idx=None) File "/usr/local/lib/python3.8/dist-packages/Klampt-0.9.0-py3.8-linux-x86_64.egg/klampt/control/robotinterface.py", line 937, in configToKlampt return model.configFromDrivers(config) File "/usr/local/lib/python3.8/dist-packages/Klampt-0.9.0-py3.8-linux-x86_64.egg/klampt/robotsim.py", line 5247, in configFromDrivers return _robotsim.RobotModel_configFromDrivers(self, driverValues) TypeError: in method 'RobotModel_configFromDrivers', argument 2 of type 'std::vector< double,std::allocator< double > > const &'

ShaoxiongYao commented 2 years ago

I think the None check condition is not appropriate, when commandedPosition is not implemented, the returned q is a list of None instead of None. We should use if all([x is not None for x in q]): if we want to check whether q is appropriate.

krishauser commented 2 years ago

Should be fixed now. Could you please check?

ShaoxiongYao commented 2 years ago

Fixed in commit 5a3f2e8f31af6eb3806bfb3854ad1c1491ddda0f.

ShaoxiongYao commented 2 years ago

Checked it is fixed using real robot.