def __init__(self,space1,space2): assert isinstance(space1,StanceCSpace) assert isinstance(space2,StanceCSpace) if len(space1.holds) > len(space2.holds): space1,space2 = space2,space1 self.space1,self.space2 = space1,space2 ClosedLoopRobotCSpace.__init__(self,space1.robot,[h.ikConstraint for h in space2.holds],space2.collider) self.addFeasibilityTest(space1.testSupportPolygon,"suppPoly")
def __init__(self, space1, space2): assert isinstance(space1, StanceCSpace) assert isinstance(space2, StanceCSpace) if len(space1.holds) > len(space2.holds): space1, space2 = space2, space1 self.space1, self.space2 = space1, space2 ClosedLoopRobotCSpace.__init__(self, space1.robot, [h.ikConstraint for h in space2.holds], space2.collider) self.addFeasibilityTest(space1.testSupportPolygon, "suppPoly")
def __init__(self,robot,holds,collider=None,world=None,checkTorqueLimits=False): if collider is None and world is not None: ignoreCollisions = [robot.link(h.link) for h in holds] collider = collide.WorldCollider(world,ignore=ignoreCollisions) ClosedLoopRobotCSpace.__init__(self,robot,[h.ikConstraint for h in holds],collider) self.holds = holds self.gravity = (0,0,-9.8) sp = contact.supportPolygon(holds) #print "Support polygon",sp self.sp = sp self.equilibriumMargin = 0.0 self.addFeasibilityTest(self.testSupportPolygon,"suppPoly") if checkTorqueLimits: raise NotImplementedError("Torque limit testing")
def __init__(self, robot, holds, collider=None, world=None, checkTorqueLimits=False): if collider is None and world is not None: ignoreCollisions = [robot.link(h.link) for h in holds] collider = collide.WorldCollider(world, ignore=ignoreCollisions) ClosedLoopRobotCSpace.__init__(self, robot, [h.ikConstraint for h in holds], collider) self.holds = holds self.gravity = (0, 0, -9.8) sp = contact.supportPolygon(holds) #print "Support polygon",sp self.sp = sp self.equilibriumMargin = 0.0 self.addFeasibilityTest(self.testSupportPolygon, "suppPoly") if checkTorqueLimits: raise NotImplementedError("Torque limit testing")
def __init__(self, robot, contacts, stabilityTestObjects=None, collider=None): """Initializes the ContactCSpace. The members contactMap, and objectives are constructed from the contacts argument. Arguments: - robot: the moving robot - contacts: a list of Holds, or ContactPoint objects with the object1 and/or object2 elements filled out. - stabilityTestObjects argument, if provided, specifies which objects are stability tested (either type RobotModel or RigidObjectModel). Otherwise, all objects and free-floating robots in contact are stability tested. """ self.gravity = (0, 0, -9.8) #construct contact structures if len(contacts) == 0: self.contactMap = dict() self.holds = [] elif isinstance(contacts[0], ContactPoint): self.contactMap = contact.contactMap( contacts, lambda x: x is None or isinstance(x, TerrainModel)) self.holds = contact.contactMapHolds(self.contactMap) else: self.holds = contacts ikobjectives = [h.ikObjective for h in self.holds] self.robotCSpace = ClosedLoopRobotCSpace(robot, ikobjectives, collider) self.objectCSpaces = [] numRobots = 0 for o in objs: if isinstance(o, RobotModel): numRobots += 1 else: self.objectCSpaces.append(RigidObjectCSpace(o, collider)) assert numRobots == 1, "ContactCSpace: Can only handle one robot in contact" #Check and create vector indexing list objs = set() for (o1, o2) in self.contactMap.iterKeys(): if hasattr(o1, 'robot'): #it's a RobotModelLink objs.insert(o1.robot()) elif o1 != None: objs.insert(o1) if hasattr(o2, 'robot'): #it's a RobotModelLink objs.insert(o2.robot()) elif o2 != None: objs.insert(o2) self.movingObjects = list(objs) if stabilityTestObjects == None: #construct from the contact list stabilityTestObjects = [] for o in objs: if isinstance(o, RobotModel): #HACK: need a better test for floating bases if o.numDrivers() + 6 <= o.numLinks(): print "ContactCSpace: Robot", o.getName( ), "is being treated as a floating-base robot" stabilityTestObjects.append(o) else: print "ContactCSpace: Robot", o.getName( ), "is being treated as a fixed-base robot" else: stabilityTestObjects.append(o) else: for o in stabilityTestObjects: assert o in objs self.stabilityTestObjects = stabilityTestObjects self.supportPolygon = None self.supportPolygonVerts = None CompositeCSpace.__init__(self, [self.robotCSpace] + self.objectCSpaces)