Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def samplePolicy(self, world):
# Get the gating condition for a specific option.
# - execution should continue until such time as this condition
# is true.
if not self.closed_loop:
return CartesianMotionPolicy(self.position,
self.rotation,
goal=self.goal), \
GoalPositionCondition(
self.goal, # what object we care about
self.position, # where we want to grab it
self.rotation,
# rotation with which we want to grab it
self.position_tolerance,
self.rotation_tolerance)
else:
obj = world.getObject(self.goal)
pg = kdl.Vector(*self.position)
Rg = kdl.Rotation.Quaternion(*self.rotation)
Tg = kdl.Frame(Rg, pg)
T = obj.state.T * Tg
position = list(T.p)
rotation = list(T.M.GetQuaternion())
return CartesianMotionPolicy(position,
rotation,
def makePolicy(self, world):
# Make the policy.
# Get the gating condition for a specific option.
# - execution should continue until such time as this condition
# is true.
return CartesianMotionPolicy(self.position,
self.rotation,
goal=self.goal), \
GoalPositionCondition(
self.goal, # what object we care about
self.position, # where we want to grab it
self.rotation,
# rotation with which we want to grab it
self.position_tolerance,
self.rotation_tolerance)