Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

report ValueError after setting movingSubset in robotplanning.makeSpace #6

#1

Klamp't version: 0.8.1
Operating System: Ubuntu 16.04 LTS

Problem Description:
Hello Dr.Hauser. During the motion planning of the UR5e with Reflex Takktile Robotic Hand, we do not want the figures of the Reflex Robotics hand to move. Therefore, when we call the function

makeSpace(world,robot,
               edgeCheckResolution=1e-2,
               extraConstraints=[],
               equalityConstraints=[],
               equalityTolerance=1e-3,
               ignoreCollisions=[],
               movingSubset=None)

in klampt.plan.robotplanning.py, we set the movingSubset=[1, 2, 3, 4, 5, 6], which only includes the 6 joints of UR5e. However, when the program is checking the

space.cspace.feasibilityFailures(qstart)

It reports ValueError:

Traceback (most recent call last):
File "/home/yuan/baymax/Test of MotionPlanning - copy.py", line 155, in
print "Start fails", space.cspace.feasibilityFailures(qstart)
File "/home/yuan/baymax/venv/local/lib/python2.7/site-packages/klampt/plan/motionplanning.py", line 410, in feasibilityFailures
return _motionplanning.CSpaceInterface_feasibilityFailures(self, q)
File "/home/yuan/baymax/venv/local/lib/python2.7/site-packages/klampt/plan/cspaceutils.py", line 143, in
self.feasibilityTests = [(lambda x,f=f:f(self.lift(x))) for f in self.ambientspace.feasibilityTests]
File "/home/yuan/baymax/venv/local/lib/python2.7/site-packages/klampt/plan/cspaceutils.py", line 156, in lift
raise ValueError("Invalid length of embedded space vector: %d should be %d"%(len(xemb),len(self.mapping)))
ValueError: Invalid length of embedded space vector: 51 should be 6

How to solve this problem? Thank you.

  • solved #5
  • replies 5
  • views 91
  • likes 0
#2

Hi Yuan,

The underlying class underlying CSpace (space.cspace) operates on the subset of moving joints. You can use space.cspace.feasibilityFailures(space.project(qstart))

#3

Thank you for your solution.
We fixed the problem after we change the code to:

print "Start fails", space.cspace.feasibilityFailures(space.project(qstart))
    print "Goal fails", space.cspace.feasibilityFailures(space.project(qgoal))

and

planner.setEndpoints(space.project(qstart), space.project(qgoal))

However, we cannot call the function:

path = gen_space.discretizePath(path)

Because I think that the gen_space is an instance of class EmbeddedCSpace(space,subset,xinit=robot.getConfig()) now, but not class robotcspace.ClosedLoopRobotCSpace(robot,equalityConstraints,collider). Then we have the change this line to:

path = gen_space.liftPath(path)

Then we find that the path of motion planning cannot fix the orientation anymore. How to fix the orientation of the robot to let its orientation can only be changed on the Z axis and set the movingSubset in makeSpace()? Thank you.

#4

Hi Yuan, as I described to you in person, I'm working on convenience functions to make this easy, but the workaround is as follows. gen_space.ambientspace is properly the ClosedLoopRobotCSpace that you had before. So you can do

path = gen_space.ambientspace.discretizePath(gen_space.liftPath(path))
#5

I did look into this more carefully, and it does look like there will be problems keeping the constraint using the embedded + closed loop constraints because of various reasons (the configuration sampler in the ambient space allows the other joints to vary).

Version 0.8.2 will fix these problems, and if you have built from source you should be able to switch to the 0.8.2 branch and re-build. But a workaround for 0.8.1 is as follows. Before the planning begins, include:

for i in range(robot.numLinks()):
    if i not in gen_space.mapping:
        gen_space.ambientspace.bound[i] = (gen_space.xinit[i],gen_space.xinit[i])
blackjack1815 accepted post #5 as the answer
#6