def __init__(self, robot, collider=None):
"""Arguments:
- robot: the robot which should move.
- collider: optional: a robotcollide.WorldCollider instance containing
the world in which the robot lives. Any ignored collisions will be
respected in the collision checker.
"""
CSpace.__init__(self)
self.robot = robot
self.bound = zip(*robot.getJointLimits())
self.collider = collider
self.addFeasibilityTest(lambda x: self.inJointLimits(x),
"joint limits")
#TODO explode these into individual self collision / env collision
#tests
def setconfig(x):
self.robot.setConfig(x)
return True
if collider:
self.addFeasibilityTest(setconfig, "dummy")
self.addFeasibilityTest(lambda x: not self.selfCollision(),
"self collision")
self.addFeasibilityTest(lambda x: not self.envCollision(),
"env collision")
self.properties['geodesic'] = 1
volume = 1
for b in self.bound:
if b[0] != b[1]: volume *= b[1] - b[0]
self.properties['volume'] = volume
def __init__(self,spaces):
CSpace.__init__(self)
self.spaces = spaces
#construct optional methods
def sampleneighborhood(self,c,r):
return self.join(s.sampleNeighborhood(cs,r) for (s,cs) in zip(self.spaces,self.split(c)))
def visible(self,a,b):
return all(s.visible(ai,bi) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b)))
def distance(self,a,b):
return sum(s.distance(ai,bi) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b)))
def interpolate(self,a,b,u):
return self.join(s.interpolate(ai,bi,u) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b)))
if any(hasattr(s,'sampleneighborhood') for s in spaces):
for s in self.spaces:
if not hasattr(s,'sampleneighborhood'):
s.sampleneighborhood = defaultsampleneighborhood
self.sampleneighborhood = sampleneighborhood
if any(hasattr(s,'visible') for s in spaces):
for s in self.spaces:
if not hasattr(s,'visible'):
s.visible = defaultvisible
self.visible = visible
if any(hasattr(s,'distance') for s in spaces):
for s in self.spaces:
if not hasattr(s,'distance'):
s.distance = defaultdistance
self.distance = distance
if any(hasattr(s,'interpolate') for s in spaces):
for s in self.spaces:
if not hasattr(s,'interpolate'):
s.interpolate = defaultinterpolate
self.interpolate = interpolate
def __init__(self,robot,collider=None):
"""Arguments:
- robot: the robot which should move.
- collider: optional: a robotcollide.WorldCollider instance containing
the world in which the robot lives. Any ignored collisions will be
respected in the collision checker.
"""
CSpace.__init__(self)
self.robot = robot
self.bound = zip(*robot.getJointLimits())
self.collider = collider
self.addFeasibilityTest(lambda x: self.inJointLimits(x),"joint limits")
#TODO explode these into individual self collision / env collision
#tests
def setconfig(x):
self.robot.setConfig(x)
return True
if collider:
self.addFeasibilityTest(setconfig,"dummy")
self.addFeasibilityTest(lambda x : not self.selfCollision(),"self collision")
self.addFeasibilityTest(lambda x: not self.envCollision(),"env collision")
self.properties['geodesic'] = 1
volume = 1
for b in self.bound:
if b[0] != b[1]: volume *= b[1]-b[0]
self.properties['volume'] = volume
def __init__(self,ambientspace,subset,xinit=None):
CSpace.__init__(self)
self.ambientspace = ambientspace
n = len(ambientspace.sample())
self.mapping = subset
#start at the zero config if no initial configuration is given
if xinit==None:
self.xinit = [0.0]*n
else:
self.xinit = xinit
#construct optional methods
def sampleneighborhood(c,r):
return self.project(self.ambientspace.sampleneighborhood(self.lift(c),r))
def visible(a,b):
return self.ambientspace.visible(self.lift(a),self.lift(b))
def distance(a,b):
return self.ambientspace.distance(self.lift(a),self.lift(b))
def interpolate(a,b,u):
return self.project(self.ambientspace.interpolate(self.lift(a)),self.lift(b))
if hasattr(ambientspace,'sampleneighborhood'):
self.sampleneighborhood = sampleneighborhood
if hasattr(ambientspace,'visible'):
self.visible = visible
if hasattr(ambientspace,'distance'):
self.distance = distance
if hasattr(ambientspace,'interpolate'):
self.interpolate = interpolate
self.eps = self.ambientspace.eps
self.bound = [self.ambientspace.bound[i] for i in self.mapping]
self.properties = self.ambientspace.properties
if self.ambientspace.feasibilityTests is not None:
self.feasibilityTests = [(lambda x:f(self.lift(x))) for f in self.ambientspace.feasibilityTests]
self.feasibilityTestNames = [(lambda x:f(self.lift(x))) for f in self.ambientspace.feasibilityTestNames]
def __init__(self,rigidObject,collider=None,translationDomain=None,rotationDomain=None):
"""
Args:
rigidObject (RigidObjectModel): the object that should move.
collider (:class:`WorldCollider`, optional): a collider instance
containing the world in which the object lives. Any ignored
collisions will be respected in the feasibility test.
translationDomain (list of pairs, optional): a bounding box in
which the translation should be sampled. If None (default),
the improper logarithmic prior is used to sample translations.
rotationDomain (pair, optional): If provided, must be a
(rotation0,rdomain) pair specifying a range in which the
rotation should be sampled. rotation0 must be an SO3 element.
rdomain may be:
* A number: rotation is sampled with absolute angular error
from rotation0 in the range [0,rdomain].
* A triple: rotation is sampled with euler angles with roll
in the range [-rdomain[0],rdomain[0]], pitch in the range
[-rdomain[1],rdomain[1]], and yaw in the range
[-rdomain[2],rdomain[2]]. The sampled rotation is then
multiplied by rotation0.
"""
CSpace.__init__(self)
self.rigidObject = rigidObject
if translationDomain is None:
translationDomain = [(-float('inf'),float('inf'))]*3
self.bound = translationDomain + [(-math.pi,math.pi)]*3
self.rotationDomain = rotationDomain
self.collider = collider
self.rotationWeight = 1.0/math.pi
if collider:
def robCollide(r):
return any(True for _ in self.collider.robotObjectCollisions(r,self.rigidObject.index))
def objCollide(o):
return any(True for _ in self.collider.objectObjectCollisions(self.rigidObject.index,o))
def terrCollide(o):
return any(True for _ in self.collider.objectTerrainCollisions(self.rigidObject.index,o))
self.addFeasibilityTest(self.setConfig,"setconfig")
self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig")
#self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision")
for o in range(self.collider.world.numRobots()):
self.addFeasibilityTest((lambda x,o=o: not robCollide(o)),"robot collision "+str(o)+" "+self.collider.world.robot(o).getName(),dependencies="setconfig")
for o in range(self.collider.world.numRigidObjects()):
if o != self.rigidObject:
self.addFeasibilityTest((lambda x,o=o: not objCollide(o)),"obj collision "+str(o)+" "+self.collider.world.rigidObject(o).getName(),dependencies="setconfig")
for o in range(self.collider.world.numTerrains()):
self.addFeasibilityTest((lambda x,o=o: not terrCollide(o)),"terrain collision "+str(o)+" "+self.collider.world.terrain(o).getName(),dependencies="setconfig")
else:
self.addFeasibilityTest(self.setConfig,"setconfig")
self.properties['geodesic'] = 1
def __init__(self,robot,collider=None):
"""
Args:
robot (RobotModel): the robot which should move.
collider (:class:`WorldCollider`, optional): a collide.WorldCollider
instance instantiated with the world in which the robot lives.
Any ignored collisions in the collider will be respected in the
feasibility tests of this CSpace.
"""
CSpace.__init__(self)
self.robot = robot
self.setBounds(zip(*robot.getJointLimits()))
self.collider = collider
self.addFeasibilityTest((lambda x: self.inJointLimits(x)),"joint limits")
def setconfig(x):
self.robot.setConfig(x)
return True
if collider:
bb0 = ([float('inf')]*3,[float('-inf')]*3)
bb = [bb0[0],bb0[1]]
def calcbb(x):
bb[0] = bb0[0]
bb[1] = bb0[1]
for i in xrange(self.robot.numLinks()):
g = self.robot.link(i).geometry()
if not g.empty():
bbi = g.getBB()
bb[0] = [min(a,b) for (a,b) in zip(bb[0],bbi[0])]
bb[1] = [max(a,b) for (a,b) in zip(bb[1],bbi[1])]
return True
def objCollide(o):
obb = self.collider.world.rigidObject(o).geometry().getBB()
if not collide.bb_intersect(obb,bb): return False
return any(True for _ in self.collider.robotObjectCollisions(self.robot.index,o))
def terrCollide(o):
obb = self.collider.world.terrain(o).geometry().getBB()
if not collide.bb_intersect(obb,bb): return False
return any(True for _ in self.collider.robotTerrainCollisions(self.robot.index,o))
self.addFeasibilityTest(setconfig,"setconfig")
self.addFeasibilityTest(calcbb,"calcbb",dependencies="setconfig")
self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig")
#self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision")
for o in range(self.collider.world.numRigidObjects()):
self.addFeasibilityTest((lambda x,o=o: not objCollide(o)),"obj collision "+str(o)+" "+self.collider.world.rigidObject(o).getName(),dependencies="calcbb")
for o in range(self.collider.world.numTerrains()):
self.addFeasibilityTest((lambda x,o=o: not terrCollide(o)),"terrain collision "+str(o)+" "+self.collider.world.terrain(o).getName(),dependencies="calcbb")
else:
self.addFeasibilityTest(setconfig,"setconfig")
self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig")
self.properties['geodesic'] = 1
def __init__(self, spaces):
CSpace.__init__(self)
self.spaces = spaces
#construct optional methods
def sampleneighborhood(self, c, r):
return self.join(
s.sampleNeighborhood(cs, r)
for (s, cs) in zip(self.spaces, self.split(c)))
def visible(self, a, b):
return all(
s.visible(ai, bi)
for (s, ai,
bi) in zip(self.spaces, self.split(a), self.split(b)))
def distance(self, a, b):
return sum(
s.distance(ai, bi)
for (s, ai,
bi) in zip(self.spaces, self.split(a), self.split(b)))
def interpolate(self, a, b, u):
return self.join(
s.interpolate(ai, bi, u)
for (s, ai,
bi) in zip(self.spaces, self.split(a), self.split(b)))
if any(hasattr(s, 'sampleneighborhood') for s in spaces):
for s in self.spaces:
if not hasattr(s, 'sampleneighborhood'):
s.sampleneighborhood = defaultsampleneighborhood
self.sampleneighborhood = sampleneighborhood
if any(hasattr(s, 'visible') for s in spaces):
for s in self.spaces:
if not hasattr(s, 'visible'):
s.visible = defaultvisible
self.visible = visible
if any(hasattr(s, 'distance') for s in spaces):
for s in self.spaces:
if not hasattr(s, 'distance'):
s.distance = defaultdistance
self.distance = distance
if any(hasattr(s, 'interpolate') for s in spaces):
for s in self.spaces:
if not hasattr(s, 'interpolate'):
s.interpolate = defaultinterpolate
self.interpolate = interpolate
def __init__(self, ambientspace, subset, xinit=None):
CSpace.__init__(self)
self.ambientspace = ambientspace
n = len(ambientspace.sample())
self.mapping = subset
#start at the zero config if no initial configuration is given
if xinit == None:
self.xinit = [0.0] * n
else:
self.xinit = xinit
#construct optional methods
def sampleneighborhood(c, r):
return self.project(
self.ambientspace.sampleneighborhood(self.lift(c), r))
def visible(a, b):
return self.ambientspace.visible(self.lift(a), self.lift(b))
def distance(a, b):
return self.ambientspace.distance(self.lift(a), self.lift(b))
def interpolate(a, b, u):
return self.project(self.ambientspace.interpolate(self.lift(a)),
self.lift(b))
if hasattr(ambientspace, 'sampleneighborhood'):
self.sampleneighborhood = sampleneighborhood
if hasattr(ambientspace, 'visible'):
self.visible = visible
if hasattr(ambientspace, 'distance'):
self.distance = distance
if hasattr(ambientspace, 'interpolate'):
self.interpolate = interpolate
self.eps = self.ambientspace.eps
self.bound = [self.ambientspace.bound[i] for i in self.mapping]
self.properties = self.ambientspace.properties
if self.ambientspace.feasibilityTests is not None:
self.feasibilityTests = [
(lambda x: f(self.lift(x)))
for f in self.ambientspace.feasibilityTests
]
self.feasibilityTestNames = [
(lambda x: f(self.lift(x)))
for f in self.ambientspace.feasibilityTestNames
]
def sample(self):
"""Overload this to implement custom sampling strategies or to handle
non-standard joints. This one will handle spin joints and
rotational axes of floating bases."""
res = CSpace.sample(self)
for i,x in enumerate(res):
if math.isnan(x):
res[i] = random.uniform(0,math.pi*2.0)
return res
def plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}):
with oracle.state_saver():
oracle.set_all_object_poses(obstacle_poses)
oracle.set_robot_config(end_config)
base_trans = get_trans(oracle.robot)
oracle.set_robot_config(start_config)
open_gripper(oracle)
if holding is not None:
oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp))
grab(oracle, holding.object_name)
traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans))
if holding is not None:
release(oracle, holding.object_name)
return traj
def plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}):
with oracle.state_saver():
oracle.set_all_object_poses(obstacle_poses)
oracle.set_robot_config(end_config)
base_trans = get_trans(oracle.robot)
oracle.set_robot_config(start_config)
open_gripper(oracle)
if holding is not None:
oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp))
grab(oracle, holding.object_name)
traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans))
if holding is not None:
release(oracle, holding.object_name)
return traj
def sampleneighborhood(c, r):
return solveManifold(
self.lift(CSpace.sampleneighborhood(self, c, r)))
def __init__(self):
CSpace.__init__(self)
AdaptiveZeroTester.__init__(self)
def sample():
xseed = self.lift(CSpace.sample(self))
return self.project(self.ambientspace.solveConstraints(xseed))
def __init__(self,
rigidObject,
collider=None,
translationDomain=None,
rotationDomain=None):
"""Arguments:
- rigidObject: the RigidObjectModel that should move.
- collider (optional): a collide.WorldCollider instance containing
the world in which the robot lives. Any ignored collisions will be
respected in the collision checker.
- translationDomain: None, or a bounding box in which the translation should be sampled.
If None (default), the Jeffrey's prior is used to sample translations.
- rotationDomain: None, or a (rotation0,rdomain) pair specifying a range in which the
rotation should be sampled. If rdomain is a number, the rotation is sampled with absolute
angular error from rotation0 in the range [0,rdomain]. If rdomain is a triple, then the
rotation is sampled with euler angles with roll in the range [-rdomain[0],rdomain[0]], pitch
in the range [-rdomain[1],rdomain[1]], and yaw in the range [-rdomain[2],rdomain[2]]. The
sampled rotation is then multiplied by rotation0.
"""
CSpace.__init__(self)
self.rigidObject = rigidObject
if translationDomain is None:
translationDomain = [(-float('inf'), float('inf'))] * 3
self.bound = translationDomain + [(-math.pi, math.pi)] * 3
self.rotationDomain = rotationDomain
self.collider = collider
self.rotationWeight = 1.0 / math.pi
if collider:
def robCollide(r):
return any(True for _ in self.collider.robotObjectCollisions(
r, self.rigidObject.index))
def objCollide(o):
return any(True for _ in self.collider.objectObjectCollisions(
self.rigidObject.index, o))
def terrCollide(o):
return any(True for _ in self.collider.objectTerrainCollisions(
self.rigidObject.index, o))
self.addFeasibilityTest(setconfig, "setconfig")
self.addFeasibilityTest((lambda x: not self.selfCollision()),
"self collision",
dependencies="setconfig")
#self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision")
for o in range(self.collider.world.numRobots()):
self.addFeasibilityTest((lambda x, o=o: not robCollide(o)),
"robot collision " + str(o) + " " +
self.collider.world.robot(o).getName(),
dependencies="setconfig")
for o in range(self.collider.world.numRigidObjects()):
if o != self.rigidObject:
self.addFeasibilityTest(
(lambda x, o=o: not objCollide(o)),
"obj collision " + str(o) + " " +
self.collider.world.rigidObject(o).getName(),
dependencies="setconfig")
for o in range(self.collider.world.numTerrains()):
self.addFeasibilityTest(
(lambda x, o=o: not terrCollide(o)),
"terrain collision " + str(o) + " " +
self.collider.world.terrain(o).getName(),
dependencies="setconfig")
else:
self.addFeasibilityTest(self.setConfig, "setconfig")
self.properties['geodesic'] = 1
def __init__(self):
CSpace.__init__(self)
AdaptiveZeroTester.__init__(self)
def sampleneighborhood(c, r):
xseed = self.lift(CSpace.sampleneighborhood(self, c, r))
return self.project(self.ambientspace.solveConstraints(xseed))
def sample():
return solveManifold(self.lift(CSpace.sample(self)))
def __init__(self,
rigidObject,
collider=None,
translationDomain=None,
rotationDomain=None):
"""
Args:
rigidObject (RigidObjectModel): the object that should move.
collider (:class:`WorldCollider`, optional): a collider instance
containing the world in which the object lives. Any ignored
collisions will be respected in the feasibility test.
translationDomain (list of pairs, optional): a bounding box in
which the translation should be sampled. If None (default),
the improper logarithmic prior is used to sample translations.
rotationDomain (pair, optional): If provided, must be a
(rotation0,rdomain) pair specifying a range in which the
rotation should be sampled. rotation0 must be an SO3 element.
rdomain may be:
* A number: rotation is sampled with absolute angular error
from rotation0 in the range [0,rdomain].
* A triple: rotation is sampled with euler angles with roll
in the range [-rdomain[0],rdomain[0]], pitch in the range
[-rdomain[1],rdomain[1]], and yaw in the range
[-rdomain[2],rdomain[2]]. The sampled rotation is then
multiplied by rotation0.
"""
CSpace.__init__(self)
self.rigidObject = rigidObject
if translationDomain is None:
translationDomain = [(-float('inf'), float('inf'))] * 3
self.bound = translationDomain + [(-math.pi, math.pi)] * 3
self.rotationDomain = rotationDomain
self.collider = collider
self.rotationWeight = 1.0 / math.pi
if collider:
def robCollide(r):
return any(True for _ in self.collider.robotObjectCollisions(
r, self.rigidObject.index))
def objCollide(o):
return any(True for _ in self.collider.objectObjectCollisions(
self.rigidObject.index, o))
def terrCollide(o):
return any(True for _ in self.collider.objectTerrainCollisions(
self.rigidObject.index, o))
self.addFeasibilityTest(self.setConfig, "setconfig")
for o in range(self.collider.world.numRobots()):
self.addFeasibilityTest((lambda x, o=o: not robCollide(o)),
"robot collision " + str(o) + " " +
self.collider.world.robot(o).getName(),
dependencies="setconfig")
for o in range(self.collider.world.numRigidObjects()):
if o != self.rigidObject.index:
self.addFeasibilityTest(
(lambda x, o=o: not objCollide(o)),
"obj collision " + str(o) + " " +
self.collider.world.rigidObject(o).getName(),
dependencies="setconfig")
for o in range(self.collider.world.numTerrains()):
self.addFeasibilityTest(
(lambda x, o=o: not terrCollide(o)),
"terrain collision " + str(o) + " " +
self.collider.world.terrain(o).getName(),
dependencies="setconfig")
else:
self.addFeasibilityTest(self.setConfig, "setconfig")
self.properties['geodesic'] = 1
queue_size=100,
slop=0.1)
ts.registerCallback(callback)
control_pub = rospy.Publisher('Wheel', Float64MultiArray, queue_size=10)
record_param_pub = rospy.Publisher('record_inner_param',
Float64StampedMultiArray,
queue_size=10)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
#--- navigation and trigger, but now only fix and trigger
global path_lst, in_planning
cspace = CSpace(h=H_START, di=D_START)
path_lst = cspace.path # list of point (d,a,alpha,back_alpha,theta,t)
path_lst = [cspace.tuple3_to_tuple6(p) for p in path_lst]
path_lst = path_lst[::-1]
in_planning = True
#---
try:
listener()
except Exception as e:
print(e)
finally:
print('hahahahahahahaha, no problem, I should be on the stair!')
