def getClosestConfig(self, robot, target, iterations, c, numsteps):
cost = 9999
res = None
start = robot.getConfig()
#goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target])
#goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target])
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
print "Active DOFs:", s.getActiveDofs()
for i in range(iterations):
#if ik.solve(goal):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
print "*****************************", end
qmin, qmax = robot.getJointLimits()
#print qmin
#print qmax
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
print "u is:", u
q = robot.interpolate(end, start, u)
#q = end
print "interpolated q is:", q
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
print "res is:", res
return res
def relevance(problem):
feasible = problem.concept()
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
env_relevance = np.zeros((W, H))
#try including some adjacent features
#footprint = [(0,0),(-1,0),(1,0),(0,-1),(0,1)]
footprint = []
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
x = (int(x[0]), int(x[1]))
if feasible or problem.env[x]:
env_relevance[x] = 1
for (dx, dy) in footprint:
if 0 <= x[0] + dx < W and 0 <= x[1] + dy < H:
env_relevance[x[0] + dx, x[1] + dy] = 1
if hierarchical:
features = [1] * 2 + [1] * 2
h = build_hierarchy(env_relevance)
for m in h:
features += [int(v) for v in m.flatten().tolist()]
return features
return [1] * 2 + [1] * 2 + [
int(v) for v in env_relevance.flatten().tolist()
]
def distBetwLinks(self, index1, index2):
'''
returns the distance between 2 links in centimeters
'''
robot = self.world.robot(0)
link1 = robot.link(index1).getWorldPosition((0, 0, 0))
link2 = robot.link(index2).getWorldPosition((0, 0, 0))
return vectorops.distance(link1, link2) * 100
def bisection(collider, robot, qstart, qgoal):
distance = vectorops.distance(qstart, qgoal)
epsilon = math.radians(2)
numsteps = int(math.ceil(distance / epsilon))
return bisectionhelper(collider, robot, qstart, qgoal, 0, numsteps,
numsteps)
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):
# interpolate path linearly between two endpoints
if path == None:
print "sending empty path"
return False
for smoothIter in range(maxSmoothIters):
# path = path
smoothePath = [0]*(len(path)*2-1)
for i in range(len(path)-1):
smoothePath[i*2] = path[i]
smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
smoothePath[-1] = path[-1]
path = smoothePath
while i <endIndex-1:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
# smooth trajectory by interpolating between two consecutive configurations
# if the distance between the two is big
# Note: might want to make dt bigger for arm movements (only 7 configurations vs 52)
if dt>0.1:
qInterp = vectorops.div(vectorops.add(q, qNext), 2)
path.insert(i+1, qInterp)
endIndex +=1
continue
else:
i += 1
waitForMove()
if counter%internalSpeed == 0 or INCREMENTAL:
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(q)
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(q)
pass
else:
#print 'milestone #', i, q
#CONTROLLER.appendMilestone(q)
pass
counter +=1
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(path[-1])
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(path[-1])
pass
else:
pass
def incremental(self, x, u):
e = self.space.interpolator(x, u)
tmax = u[0]
t = 0
c = 0
while t < tmax:
t = min(tmax, t + self.timestep)
xnext = e.eval(t / tmax)
c += vectorops.distance(x, xnext)
x = xnext
return c
def getClosestConfig(self, robot, target, iterations, c, numsteps):
""" given a target object position, returns a configuration with end effector close to the target object but without colliding with it"""
cost = 9999
res = None
start = robot.getConfig()
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
for i in range(iterations):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
qmin, qmax = robot.getJointLimits()
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
q = robot.interpolate(end, start, u)
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
return res
def selfcollision(robot, qstart, qgoal):
"""returns whether a self collision is predicted along the route from qstart to qgoal"""
distance = vectorops.distance(qstart, qgoal)
epsilon = math.radians(2)
numsteps = int(math.ceil(distance / epsilon))
for i in xrange(numsteps + 1):
u = float(i) / numsteps
q = robot.interpolate(qstart, qgoal, u)
robot.setConfig(q)
if robot.selfCollides():
return True
return False
def obstaclecollision(collider, robot, qstart, qgoal):
"""returns whether a obstacle collision is predicted along the route from qstart to qgoal"""
#return None
distance = vectorops.distance(qstart, qgoal)
epsilon = math.radians(2)
numsteps = int(math.ceil(distance / epsilon))
print "numTerrains:", collider.world.numTerrains()
for i in xrange(numsteps + 1):
u = float(i) / numsteps
q = robot.interpolate(qstart, qgoal, u)
robot.setConfig(q)
collisions = collider.robotTerrainCollisions(robot.index)
if any(collisions):
return collisions
return None
def relevance(problem):
feasible = problem.concept()
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
env_relevance = np.zeros(workspace_size)
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
for (a, b, c) in sphere_collision_tests(x, robot_radius,
problem.env):
if feasible or problem.env[(a, b, c)]:
env_relevance[(a, b, c)] = 1
return [1] * 3 + [1] * 3 + env_relevance.flatten().tolist()
def concept(problem): # compute y
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
collisions = 0
#hacky "rasterization" by marching down segment
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
i / nsteps)
if sphere_collision(x, robot_radius, problem.env):
# colliding
return 0
#collision free
return 1
def apply_tendon_forces(self,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 100000.0
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p1w = se3.apply(b1.getTransform(),self.tendon1_local)
p2w = se3.apply(b2.getTransform(),self.tendon2_local)
d = vectorops.distance(p1w,p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w,p1w))
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
b1.applyForceAtPoint(vectorops.mul(direction,f),p1w)
b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w)
else:
pass
def concept(problem):
d = vectorops.distance(problem.start, problem.goal)
h = 0.2
nsteps = int(math.ceil(d / h))
collisions = 0
#hacky "rasterization" by marching down segment
for i in range(nsteps + 1):
if nsteps == 0:
x = problem.start
else:
x = vectorops.interpolate(problem.start, problem.goal,
float(i) / nsteps)
if problem.env[int(x[0]), int(x[1])]:
collisions += 1
if collisions >= label_obstacle_threshold:
return 0
#collision free
return 1
def apply_tendon_forces(self, i, link1, link2, rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(
self.model.robot.link(self.model.proximal_links[0] - 3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(), self.tendon0_local)
p1w = se3.apply(b1.getTransform(), self.tendon1_local)
p2w = se3.apply(b2.getTransform(), self.tendon2_local)
d = vectorops.distance(p1w, p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w, p1w))
f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d -
rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f, 100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w, p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w))
pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0))
tangential_axis = vectorops.cross(pulley_axis, pulley_direction)
cosangle = vectorops.dot(straight, tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1])
b0.applyForceAtLocalPoint(
vectorops.mul(base_direction, -f),
vectorops.madd(p0w, base_direction, 0.04))
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, cosangle * f),
self.tendon1_local)
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, -cosangle * f),
self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction, -f),
self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f)
self.forces[i][2] = vectorops.mul(direction, f)
else:
self.forces[i] = [None, None, None]
return
def apply_tendon_forces(self,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 100000.0
b1 = self.sim.getBody(self.model.robot.getLink(link1))
b2 = self.sim.getBody(self.model.robot.getLink(link2))
p1w = se3.apply(b1.getTransform(),self.tendon1_local)
p2w = se3.apply(b2.getTransform(),self.tendon2_local)
d = vectorops.distance(p1w,p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w,p1w))
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
#print "d",d,"rest length",rest_length,"force",f
b1.applyForceAtPoint(vectorops.mul(direction,f),p1w)
b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w)
else:
#print "d",d,"rest length",rest_length
pass
def dubinsCarTest():
cspace = Geometric2DCSpace()
vspace = BoxConfigurationSpace([-1, -1], [1, 1])
start = [0.5, 0.3, 0]
goal = [0.5, 0.7, 0]
controlSpace = DubinsCarSpace(MultiConfigurationSpace(cspace, SO2Space()))
controlSpace.setDistanceBounds(-0.25, 0.25)
controlSpace.setTurnRateBounds(-3.14, 3.14)
numControlsPerSample = 1
if numControlsPerSample > 1:
controlSpace = RepeatedControlSpace(controlSpace, numControlsPerSample)
objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample)
goalRadius = 0.1
return PlanningProblem(
controlSpace,
start,
goal,
objective=objective,
visualizer=DubinsVisualizer(cspace),
goalRadius=goalRadius,
costLowerBound=lambda x, y: vectorops.distance(x[:2], y[:2]))
def generate_trajectory(qi, qf):
i = 0
endIndex = 2
path = [0.0, 0.0]
path[0] = qi
path[1] = qf
print qi, qf
while i < endIndex-1:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
# smooth trajectory by interpolating between two consecutive configurations
# if the distance between the two is big
if dt>0.1:
qInterp = vectorops.div(vectorops.add(q, qNext), 2)
path.insert(i+1, qInterp)
endIndex +=1
continue
else:
i += 1
print len(path)
return path
def distance(self,a,b):
return vectorops.distance(a,b)
def distance(self, a, b):
#TODO: return a distance metric between a and b that correctly
#takes rotation into account. Current implementation assumes
#a cartesian space
return vectorops.distance(a, b)
def contains(self, point):
return (vectorops.distance(point, self.center) <= self.radius)
def euclideanMetric(a, b):
return vectorops.distance(a, b)
def length(self):
return vectorops.distance(self.a, self.b)
def distance(self,a,b):
#TODO: return a distance metric between a and b that correctly
#takes rotation into account. Current implementation assumes
#a cartesian space
return vectorops.distance(a,b)
def contains(self,point):
return (vectorops.distance(point,self.center) <= self.radius)
def euclideanMetric(a,b):
return vectorops.distance(a,b)
def length(self):
return vectorops.distance(self.a,self.b)
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):
# interpolate path linearly between two endpoints
if path == None:
print "sending empty path"
return False
# n = self.robot.numLinks()
# for i in range(len(path)):
# # if we specify a limb, and the path only has seven numbers (DOF for each arm, we shouldn't append 0's)
# if readConstants and limb is not None:
# # if we're reading a path from the milestones
# pass
# else:
# #print path
# if len(path[i])<n:
# path[i] += [0.0]*(n-len(path[i]))
# #pass
for smoothIter in range(maxSmoothIters):
# path = path
smoothePath = [0]*(len(path)*2-1)
for i in range(len(path)-1):
smoothePath[i*2] = path[i]
smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
smoothePath[-1] = path[-1]
path = smoothePath
i = 0
endIndex = len(path)
# if endIndex==1:
# q=path[0]
# path[0]=self.robot.getConfig()
# path.append(q)
counter = 0
#path.append(path[-1])
#endIndex = len(path)
#print 'myPath = ', path
speed = 1
while i+1 <endIndex:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
dt = dt / speed
i += 1
waitForMove()
# if INCREMENTAL:
# time.sleep(.1)
if limb == 'left':
CONTROLLER.appendMilestoneLeft(q,dt)
#pass
elif limb == 'right':
CONTROLLER.appendMilestoneRight(q,dt)
#pass
else:
#print 'milestone #', i, q
#CONTROLLER.appendMilestone(q)
CONTROLLER.appendLinear(q,dt)
#pass
"""
