def add_multiple_lights(properties,object,dist,numLight,gravity=[0,0,-9.81],tgt=None,color=[1.,1.,1.], \
spotlight=False,radius=15.,falloff=20.,tightness=10., \
area=0.,sample=9,adaptive=True,jitter=True):
"""This is a convenient function that calls add_light multiple times"""
#normalize gravity
g = op.mul(gravity, -1 / op.norm(gravity))
#compute frame
gabs = [abs(gi) for gi in g]
id = gabs.index(min(gabs))
t0 = [1. if i == id else 0. for i in range(3)]
t1 = op.cross(t0, g)
t1 = op.mul(t1, 1 / op.norm(t1))
t0 = op.cross(t1, g)
#find highest direction
bb = compute_bb(object)
ctr = op.mul(op.add(bb[0], bb[1]), 0.5)
distg = sum([abs((bb[1][i] - bb[0][i]) / 2 * g[i]) for i in range(3)])
#add each light
for i in range(numLight):
angle = math.pi * 2 * i / numLight
d0 = op.mul(g, distg)
d1 = op.mul(t0, math.sin(angle) * dist)
d2 = op.mul(t1, math.cos(angle) * dist)
add_light(properties, op.add(d0, op.add(d1,
d2)), ctr, color, spotlight,
radius, falloff, tightness, area, sample, adaptive, jitter)
def getCommandVelocity(self, q, dq, dt):
"""Gets the command velocity from the current state of the
robot.
"""
eP = self.getSensedError(q)
#vcmd = hP*eP + hD*eV + hI*eI
vP = gen_mul(self.hP, eP)
vcmd = vP
#D term
vcur = self.getSensedVelocity(q, dq, dt)
eD = None
if vcur != None:
eD = vectorops.sub(vcur, self.dxdes)
vD = gen_mul(self.hD, eD)
vcmd = vectorops.add(vcmd, vD)
#I term
if self.eI != None:
vI = gen_mul(self.hI, self.eI)
vcmd = vectorops.add(vcmd, vI)
#print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI
#do acceleration limiting
if vcur != None:
dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt)
dvnorm = vectorops.norm(dvcmd)
if dvnorm > self.dvcmdmax:
vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt)
print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd
#do velocity limiting
vnorm = vectorops.norm(vcmd)
if vnorm > self.vcmdmax:
vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm)
print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd
return vcmd
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def get_left_dir(self, zeroZ=False):
dir = op.cross([0, 0, 1], self.get_camera_dir())
if zeroZ:
dir = (dir[0], dir[1], 0)
if op.norm(dir) > 1e-6:
dir = op.mul(dir, 1 / op.norm(dir))
return dir
def get_camera_dir(self, zeroZ=False):
cam = self.view.camera
dir = op.sub(cam.tgt, self.get_camera_pos())
if zeroZ:
dir = (dir[0], dir[1], 0)
if op.norm(dir) > 1e-6:
dir = op.mul(dir, 1 / op.norm(dir))
dir[1] *= -1
return dir
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1):
"""For a 2R arm centered at the origin, solves for the joint angles
(q1,q2) that places the end effector at (x,y).
The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')].
"""
D = vectorops.norm((x,y))
thetades = math.atan2(y,x)
if D == 0:
raise ValueError("(x,y) at origin, infinite # of solutions")
c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2)
q2s = []
if c2 < -1:
print("solve_2R_inverse_kinematics: (x,y) inside inner circle")
return []
elif c2 > 1:
print("solve_2R_inverse_kinematics: (x,y) out of reach")
return []
else:
if c2 == 1:
q2s = [math.acos(c2)]
else:
q2s = [math.acos(c2),-math.acos(c2)]
res = []
for q2 in q2s:
thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2))
q1 = thetades - thetaactual
res.append((q1,q2))
return res
def distMetric(self, n1, n2):
dist = self.env.n * vectorops.norm(
vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z]))
dist += self.env.sumDist * math.fabs(n1.sc - n2.sc)
dist += 1 * self.env.sumDist * math.fabs(
n1.sc + n2.sc) / 2 * math.fabs(n1.tht - n2.tht)
return dist
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def draw_hull(hull,scale=0.01):
"""Draws a ConvexHull with 3D texture coordinates"""
if len(hull.simplices)==0:
print "Hull with no simplices?"
return
centroid = sum([hull.points[v] for v in hull.vertices],[0.0]*3) / len(hull.vertices) * scale
vmin = [min([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)]
vmax = [max([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)]
uvwscale = [1.0/(b-a) if b != a else 1.0 for (a,b) in zip(vmin,vmax)]
for simplex in hull.simplices:
ia,ib,ic = simplex
a = vectorops.mul(hull.points[ia].tolist(),scale)
b = vectorops.mul(hull.points[ib].tolist(),scale)
c = vectorops.mul(hull.points[ic].tolist(),scale)
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
if vectorops.dot(n,vectorops.sub(centroid,a)) > 0:
b,c = c,b
n = vectorops.mul(n,-1)
try:
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
except ZeroDivisionError:
pass
glBegin(GL_TRIANGLES)
glTexCoord3f(uvwscale[0]*(a[0]-vmin[0]),uvwscale[1]*(a[1]-vmin[1]),uvwscale[2]*(a[2]-vmin[2]))
glVertex3f(*a)
glTexCoord3f(uvwscale[0]*(b[0]-vmin[0]),uvwscale[1]*(b[1]-vmin[1]),uvwscale[2]*(b[2]-vmin[2]))
glVertex3f(*b)
glTexCoord3f(uvwscale[0]*(c[0]-vmin[0]),uvwscale[1]*(c[1]-vmin[1]),uvwscale[2]*(c[2]-vmin[2]))
glVertex3f(*c)
glEnd()
def segment_triangle_intersection(seg, tri, umin=0, umax=1):
"""Given a 3D segment (p,q) and a triangle (a,b,c), returns the point of
intersection if the region of the segment in interval [umin,umax] intersects the
triangle. Otherwise returns None"""
p, q = seg
a, b, c = tri
d = vectorops.sub(b, a)
e = vectorops.sub(c, a)
n = vectorops.cross(d, e)
if vectorops.norm(n) < 1e-7: #degenerate
return None
n = vectorops.unit(n)
ofs = vectorops.dot(n, a)
#Solve for n^T(p + u(q-p)) = ofs
denom = vectorops.dot(n, vectorops.sub(q, p))
numer = ofs - vectorops.dot(n, p)
if abs(denom) < 1e-7: #near parallel
return None
u = numer / denom
if u < umin or u > umax:
return None
#now check whether point of intersection lies in triangle
x = vectorops.madd(p, vectorops.sub(q, p), u)
xloc = vectorops.sub(x, a)
#solve for coordinates [r,s,t] such that xloc = r*n + s*d + t*e
try:
rst = np.dot(np.linalg.inv(np.array([n, d, e]).T), xloc)
except np.linalg.linalg.LinAlgError:
return None
r, s, t = rst
assert abs(r) < 1e-4
if s >= 0 and t >= 0 and s + t <= 1:
return x
return None
def score(self,robot):
"""Returns an error score that is equal to the optimum at a feasible
solution. Evaluated at the robot's current configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
c = (0 if self.costFunction is None else self.costFunction(robot.getConfig()))
return c+vectorops.norm(solver.getResidual())
def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio):
reactive_vel = [0.0, 0.0, 0.0]
pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand))
pos_diff_norm = vectorops.norm(pos_diff)
if pos_diff_norm >= 0.02:
vel = vectorops.mul(pos_diff, vel_ratio)
reactive_vel = list(vel)
return reactive_vel
def score(self, robot):
"""Returns an error score that is equal to the optimum at a feasible
solution. Evaluated at the robot's current configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
c = (0 if self.costFunction is None else self.costFunction(
robot.getConfig()))
return c + vectorops.norm(solver.getResidual())
def interpolate_rotation(R1, R2, u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def findOptPrepareLoc(self, hit_xyz, prep_loc_dict):
best_loc = None
min_dist = float('inf')
for each_key in prep_loc_dict.keys():
cur_prep = prep_loc_dict[each_key]
cur_dist = vectorops.norm(vectorops.sub(hit_xyz, cur_prep))
if cur_dist < min_dist:
best_loc = each_key
min_dist = cur_dist
return best_loc
def look_at(self, pos, tgt, scale=None):
if self.lockX:
tgt[0] = pos[0]
if self.lockY:
tgt[1] = pos[1]
cam = self.view.camera
if scale is not None:
cam.dist = op.norm(op.sub(tgt, pos)) * scale
cam.rot = self.get_camera_rot(op.sub(pos, tgt))
cam.tgt = tgt
def add_light(properties,pos,tgt=None,color=[1.,1.,1.], \
spotlight=False,radius=15.,falloff=20.,tightness=10., \
area=0.,sample=9,adaptive=True,jitter=True):
if 'lights' not in properties:
properties['lights'] = []
if isinstance(pos, list) and isinstance(pos[0], list):
if isinstance(tgt, list) and isinstance(tgt[0], list):
for p, t in zip(pos, tgt):
add_light(properties,pos=p,tgt=t,color=color, \
spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \
area=area,sample=sample,adaptive=adaptive,jitter=jitter)
else:
for p in pos:
add_light(properties,pos=p,tgt=tgt,color=color, \
spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \
area=area,sample=sample,adaptive=adaptive,jitter=jitter)
else:
light_params = [list(pos), 'color', list(color)]
if spotlight:
light_params += [
'spotlight', 'radius', radius, 'falloff', falloff, 'tightness',
tightness, 'point_at', tgt
]
if area > 0.:
d = op.sub(tgt, pos)
d = op.mul(d, 1 / op.norm(d))
minId = None
for i in range(3):
if abs(d[i]) <= abs(d[(i + 1) % 3]) and abs(d[i]) <= abs(
d[(i + 2) % 3]):
minId = i
t0 = [0, 0, 0]
t0[minId] = 1.
t1 = op.cross(d, t0)
t1 = op.mul(t1, 1 / op.norm(t1))
t0 = op.cross(d, t1)
light_params += [
'area_light',
list(op.mul(t0, area)),
list(op.mul(t1, area)), sample, sample, 'adaptive', 1, 'jitter'
]
properties['lights'].append(vp.LightSource(*light_params))
def updateVis(self):
"""This gets called every control loop.
TODO: You may consider visually debugging some of your code here, along with initVis().
For example, to draw a ghost robot at a given configuration q, you can call:
kviz.add_ghost() (in initVis)
kviz.set_ghost(q) (in updateVis)
The current code draws gravity-inflenced arcs leading from all the
object position / velocity estimates from your state estimator. Event C
folks should set gravity=0 in the following code.
"""
#print "Start of a new time increment"
if self.objectEstimates:
for o in self.objectEstimates.objects:
x = [o.x[0], o.x[1], o.x[2]]
v = [o.x[3], o.x[4], o.x[5]]
# draw the actual spheres
kviz.update_sphere("object_est" + str(o.name), o.x[0], o.x[1],
o.x[2], 0.03)
kviz.set_color("object_est" + str(o.name), o.name[0],
o.name[1], o.name[2])
# if the ball is off the table, don't evaluate
if ((x[2] < (-.5))):
#print o.name, " is off the table"
pass
# if ball is in the corner,
# if ball is moving backwards,
# if ball's velocity is below threshold
elif (x[0] >= 3 or o.x[3] > 0 or vectorops.norm(v) < 0.5):
#print o.name, " is not valid"
pass
else:
#draw a point
#draw an arc
trace = []
#print o.name, " x_position", o.x[0], " ", o.x[1], " ", o.x[2]
gravity = 9.8
for i in range(40):
# set time interval
t = 0.05
# append to trace
trace.append(x)
# predict the next location
[x, v] = self.predict_next_location(x, v, t, gravity)
kviz.update_polyline("object_trace" + str(o.name), trace)
kviz.set_color("object_trace" + str(o.name), o.name[0],
o.name[1], o.name[2])
def myPlayerLogic(self, dt, sensorReadings, objectStateEstimate,
robotController):
"""
TODO: fill this out to updates the robot's low level controller
in response to a new time step. This is allowed to set any
attributes of MyController that you wish, such as self.state.
Arguments:
- dt: the simulation time elapsed since the last call
- sensorReadings: the sensor readings given on the current time step.
this will be a dictionary mapping sensor names to sensor data.
The name "blobdetector" indicates a sensor reading coming from the
blob detector. The name "omniscient" indicates a sensor reading
coming from the omniscient object sensor. You will not need to
use raw sensor data directly, if you have a working state estimator.
- objectStateEstimate: a MultiObjectStateEstimate class (see
stateestimation.py) produced by the state estimator.
- robotController: a SimRobotController instance giving access
to the robot's low-level controller. You can call any of the
methods. At the end of this call, you can either compute some
PID command via robotController.setPIDCommand(), or compute a
trajectory to execute via robotController.set/addMilestone().
(if you are into masochism you can use robotController.setTorque())
"""
robot = robotController.model()
# Solver
possible_catch = []
# if no result return to the center prepare location
num_obs = len(objectStateEstimate.objects)
if num_obs == 0:
self.qdes = self.ini_config
else:
for o in objectStateEstimate.objects:
if (o.meanVelocity()[0] < -1
and abs(o.meanPosition()[1]) < WALL_Y
and o.meanPosition()[0] > WALL_XS[1]
and o.meanPosition()[0] < 2) or vectorops.norm(
vectorops.sub(o.meanVelocity(), [0] * 3)) == 0:
CATCH_EST.addRecord(o)
catch_array = CATCH_EST.getRecord(o.name)
if catch_array != []:
last_record = catch_array[-1]
for idx in range(last_record.shape[0]):
possible_catch.append(last_record[idx, :].tolist())
opt_result = self.calOptCatch(robot, possible_catch)
self.qdes = opt_result[0]
robotController.setMilestone(self.qdes)
return
def svg_path_to_trajectory(p):
"""Produces either a Trajectory or HermiteTrajectory according to an SVG
Path. The path must be continuous.
"""
from svgpathtools import Path, Line, QuadraticBezier, CubicBezier, Arc
if not p.iscontinuous():
raise ValueError("Can't do discontinuous paths")
pwl = not any(isinstance(seg, (CubicBezier, QuadraticBezier)) for seg in p)
if pwl:
milestones = []
for seg in p:
a, b = seg.start, seg.end
if not milestones:
milestones.append([a.real, a.imag])
milestones.append([b.real, b.imag])
return Trajectory(list(range(len(milestones))), milestones)
times = []
milestones = []
for i, seg in enumerate(p):
if isinstance(seg, CubicBezier):
a, c1, c2, b = seg.start, seg.control1, seg.control2, seg.end
vstart = (c1.real - a.real) * 3, (c1.imag - a.imag) * 3
vend = (b.real - c2.real) * 3, (b.imag - c2.imag) * 3
if not milestones:
milestones.append([a.real, a.imag, vstart[0], vstart[1]])
times.append(i)
elif vectorops.distance(milestones[-1][2:], vstart) > 1e-4:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
milestones.append([a.real, a.imag, vstart[0], vstart[1]])
times.append(i)
milestones.append([b.real, b.imag, vend[0], vend[1]])
times.append(i + 1)
elif isinstance(seg, Line):
a, b = seg.start, seg.end
if not milestones:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
elif vectorops.norm(milestones[-1][2:]) > 1e-4:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
milestones.append([b.real, b.imag, 0, 0])
times.append(i + 1)
else:
raise NotImplementedError(
"Can't handle pieces of type {} yet".format(
seg.__class.__name__))
return HermiteTrajectory(times, milestones)
def advance(self, q, dq, dt):
""" Updates internal state: accumulates iterm and updates x_last
"""
if self.weight > 0:
eP = self.getSensedError(q)
# update iterm
if self.eI == None:
self.eI = vectorops.mul(eP, dt)
else:
self.eI = vectorops.madd(self.eI, eP, dt)
einorm = vectorops.norm(self.eI)
if einorm > self.eImax:
self.eI = vectorops.mul(self.eI, self.eImax / einorm)
#update qLast
self.qLast = q
return
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP,dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI,self.eImax/einorm) #update qLast self.qLast = q return
def force(q, target, obstacles):
"""Returns the potential field force for a robot at configuration q,
trying to reach the given target, with the specified obstacles.
Input:
- q: a 2D point giving the robot's configuration
- robotRadius: the radius of the robot
- target: a 2D point giving the robot's target
- obstacles: a list of Circle's giving the obstacles
"""
#basic target-tracking potential field implemented here
#TODO: implement your own potential field
att_force = vectorops.mul(vectorops.sub(target, q), attractiveConstant)
total_rep_force = [0, 0]
for each_ob in obstacles:
cur_rep_force = repForce(q, each_ob, repulsiveDistance)
total_rep_force = vectorops.add(total_rep_force, cur_rep_force)
total_force = vectorops.add(att_force, total_rep_force)
#limit the norm of f
if vectorops.norm(total_force) > 1:
total_force = vectorops.unit(vectorops.add(total_force,
(1e-10, 1e-10)))
return total_force
def create_env_light_for_bb(bb, distRatio=1.5, zdim=2, nLight=3):
xdim = (zdim + 1) % 3
ydim = (zdim + 2) % 3
ctr = [0., 0., 0.]
ctr[xdim] = (bb[0][xdim] + bb[1][xdim]) / 2
ctr[ydim] = (bb[0][ydim] + bb[1][ydim]) / 2
ctr[zdim] = (bb[1][zdim] - bb[0][zdim]) * distRatio + bb[1][zdim]
x = (bb[1][xdim] - bb[0][xdim]) / 2
y = (bb[1][ydim] - bb[0][ydim]) / 2
dist = op.norm((x, y)) * (1. + distRatio)
pos = []
for d in range(nLight):
ang = math.pi * 2 * d / nLight
ctrd = [c for c in ctr]
ctrd[xdim] += dist * math.cos(ang)
ctrd[ydim] += dist * math.sin(ang)
pos.append(ctrd)
tgt = [c for c in ctr]
tgt[zdim] = bb[0][zdim]
return pos, tgt
def force(q, target, obstacles):
"""Returns the potential field force for a robot at configuration q,
trying to reach the given target, with the specified obstacles.
Input:
- q: a 2D point giving the robot's configuration
- robotRadius: the radius of the robot
- target: a 2D point giving the robot's target
- obstacles: a list of Circle's giving the obstacles
"""
#basic target-tracking potential field implemented here
# calculate the attractive force due to the goal
f = vectorops.mul(vectorops.sub(target, q), attractiveConstant)
for obstacle in obstacles:
dist = obstacle.distance(q)
# only add the repulsive force if the obstacle is "within" range
if dist > repulsiveDistance:
continue
magnitude = (1.0 / dist - 1 / repulsiveDistance)
# alter the magnitude to have a repulsiveConstant and to
# divide by square of repulsiveDistance
# Dividing by the square strengthens the repulsive force
# The repuslvieConstant scales the strength of the repulsive force linearly
magnitude = repulsiveConstant * magnitude / (dist**2)
# set the direction of repulsive force away from the obstacle
direction = vectorops.unit(vectorops.sub(q, obstacle.center))
f = vectorops.madd(f, direction, magnitude)
#limit the norm of f
if vectorops.norm(f) > 1:
f = vectorops.unit(f)
return f
def localPlanner(self, n1, n2, sleepFlag=False):
dist = self.env.n * vectorops.norm(
vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z]))
vel = 0.04
tm = dist / vel
for i in range(int(tm)):
delta = i / float(tm)
x = n1.x + (n2.x - n1.x) * delta
y = n1.y + (n2.y - n1.y) * delta
z = n1.z + (n2.z - n1.z) * delta
sc = n1.sc + (n2.sc - n1.sc) * delta
tht = n1.tht + (n2.tht - n1.tht) * delta
vis.lock()
self.env.setConfig(x, y, z, sc, tht)
vis.unlock()
colFlag = False
colFlag = self.env.checkCollision()
if (colFlag):
return False
if sleepFlag:
time.sleep(vel / 2)
self.env.setConfig(n2.x, n2.y, n2.z, n2.sc, n2.tht)
return True
def myPlayerLogic(self, dt, sensorReadings, objectStateEstimate,
robotController):
"""
TODO: fill this out to updates the robot's low level controller
in response to a new time step. This is allowed to set any
attributes of MyController that you wish, such as self.state.
Arguments:
- dt: the simulation time elapsed since the last call
- sensorReadings: the sensor readings given on the current time step.
this will be a dictionary mapping sensor names to sensor data.
The name "blobdetector" indicates a sensor reading coming from the
blob detector. The name "omniscient" indicates a sensor reading
coming from the omniscient object sensor. You will not need to
use raw sensor data directly, if you have a working state estimator.
- objectStateEstimate: a MultiObjectStateEstimate class (see
stateestimation.py) produced by the state estimator.
- robotController: a SimRobotController instance giving access
to the robot's low-level controller. You can call any of the
methods. At the end of this call, you can either compute some
PID command via robotController.setPIDCommand(), or compute a
trajectory to execute via robotController.set/addMilestone().
(if you are into masochism you can use robotController.setTorque())
"""
#these are pulled out here for your convenience
qcmd = robotController.getCommandedConfig()
vcmd = robotController.getCommandedVelocity()
qsns = robotController.getSensedConfig()
vsns = robotController.getSensedVelocity()
#setting a PID command can be accomplished with the following
#robotController.setPIDCommand(self.qdes,[0.0]*7)
#queuing up linear interpolation can be accomplished with the following
#dt = 0.5 #how much time it takes for the robot to reach the target
#robotController.appendLinear(self.qdes,dt)
#queuing up a fast-as possible interpolation can be accomplished with the following
#robotController.addMilestone(self.qdes)
# get the RobotModel object from the robotController
robot = robotController.model()
solved_location = 0
# Check if a ball is within the space "reachable" by our robot.
for o in self.objectEstimates.objects:
x = [o.x[0], o.x[1], o.x[2]]
v = [o.x[3], o.x[4], o.x[5]]
#print "Object Position: ", x
#print "Object Velocity: ", v
# if the ball is off the table, don't evaluate
if ((x[2] < (-.5))):
#print o.name, " is off the table"
pass
# if ball is in the corner,
# if ball is moving backwards,
# if ball's velocity is below threshold
elif (x[0] >= 3 or o.x[3] > 0 or vectorops.norm(v) < 0.5):
#print o.name, " is not valid"
pass
else:
#print o.name, " is moving"
# define velocity for these calculations
gravity = 9.8
for i in range(20):
# set time interval
t = 0.05
# check if the trajectory falls within robot's arm
[x, v] = self.predict_next_location(x, v, t, gravity)
inBounds = self.withinReach(x, -2.5, -1.7, -1.2, 1.2, 0.09,
1.2)
if (inBounds == 1):
#print o.name, " is projected to score. Block this location at:"
#print x[0], x[1], x[2]
(q, solved) = self.ik_solver(robot, x, qsns)
self.state = "blocking"
solved_location = 1
if (solved):
#print "setting milestone for blocking robot"
q[robot.numLinks() - 1] = 0
self.lastx = x
robotController.setMilestone(q)
break
# predict the next location
# end loop
# state machine to determine which state the robot should enter
# requires 4 misses to enter blocking mode
if (solved_location == 0):
if (self.state == "blocking"):
self.state = "blocking1"
elif (self.state == "blocking1"):
self.state = "blocking2"
elif (self.state == "blocking2"):
self.state = "blocking3"
else: # if waiting or blocking 3
self.state = 'waiting'
# if the robot doesn't have any obstacles to block, go back to initial position
if self.state == 'waiting':
#print qsns
#print "waiting"
if (self.lastx[1] <= -0.29):
if (self.lastx[1] < 0.3):
xdes = [-1.5, -0.3, 0.5]
else:
xdes = [-1.5, -0.3, 0.2]
elif (self.lastx[1] >= 0.29):
if (self.lastx[1] < 0.3):
xdes = [-1.5, 0.3, 0.5]
else:
xdes = [-1.5, 0.3, 0.2]
#if(qsns[1] > 0.4 or qsns[1] < -0.4):
# xdes = [-1.2, 0, 0.5]
else:
xdes = [-1.2, 0, 0.9]
self.lastx = xdes
(q, solved) = self.ik_solver(robot, xdes, qsns)
q[robot.numLinks() - 1] = 0
if (solved):
#print "setting milestone for waiting robot"
robotController.setMilestone(q)
elif (
self.state == "reset"
): # if you're blocking, the controller was already programmed so just wait it out
pass
else:
#print "blocking"
pass
return
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,
visualize=False,verbose=0):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Side effect: the positions of objects in world are modified
Returns:
(tuple): (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:",objects_bound)
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = _get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in range(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
if verbose:
print("Sampled position of",object.getName(),object.getTransform()[1])
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
if verbose:
print("Failed to place object",object.getName())
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
if verbose:
print("Beginning to simulate")
#start letting everything fall
for firstfall in range(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
def isArrive(cur_q, target_q, error_limit):
return vectorops.norm(vectorops.sub(cur_q, target_q)) < error_limit
def predict_line(lineStarts,lineEnds,lineNormals,lineTorqueAxes,pcd,param,discretization,num_iter,queryDList,vis,world):
vision_task_1 = False#True means we are doing the collision detection.
o3d = False
#create a pcd in open3D
equilibriumPcd = open3d.geometry.PointCloud()
xyz = []
rgb = []
normals = []
for ele in pcd:
xyz.append(ele[0:3])
rgb.append(ele[3:6])
normals.append(ele[6:9])
equilibriumPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32))
equilibriumPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32))
equilibriumPcd.normals = open3d.utility.Vector3dVector(np.asarray(normals,dtype=np.float32))
# equilibriumPcd,ind=statistical_outlier_removal(equilibriumPcd,nb_neighbors=20,std_ratio=0.75)
if o3d:
vis3 = open3d.visualization.Visualizer()
vis3.create_window()
open3dPcd = open3d.geometry.PointCloud()
vis3.add_geometry(open3dPcd)
probe = world.rigidObject(0)
klampt_pcd_object = world.rigidObject(1) #this is a rigid object
klampt_pcd = klampt_pcd_object.geometry().getPointCloud() #this is now a PointCloud()
N_pts = klampt_pcd.numPoints()
dt = 0.1
for pointNumber in num_iter:
####Preprocess the pcd ####
lineStart0 = lineStarts[pointNumber]
lineEnd0 =lineEnds[pointNumber]
lineTorqueAxis = lineTorqueAxes[pointNumber]
N = 1 + round(vo.norm(vo.sub(lineStart0,lineEnd0))/discretization)
s = np.linspace(0,1,N)
lineNormal = lineNormals[pointNumber]
localXinW = vo.sub(lineEnd0,lineStart0)/np.linalg.norm(vo.sub(lineEnd0,lineStart0))
localYinW = (lineTorqueAxis)/np.linalg.norm(lineTorqueAxis)
lineStartinLocal = [0,0]
lineEndinLocal = [np.dot(vo.sub(lineEnd0,lineStart0),localXinW),
np.dot(vo.sub(lineEnd0,lineStart0),localYinW)]
#################first project the pcd to the plane of the line##############
#The start point is the origin of the plane...
projectedPcd = [] #Each point is R^10
projectedPcdLocal = [] #localXY coordinate
#the projected Pcd is in local frame....
for i in range(len(pcd)):
p = pcd[i][0:3]
projectedPt = vo.sub(p,vo.mul(lineNormal,vo.dot(vo.sub(p,lineStart0),lineNormal))) ##world origin
projectedPt2D = vo.sub(projectedPt,lineStart0)#world origin
projectedPt2DinLocal = [vo.dot(projectedPt2D,localXinW),vo.dot(projectedPt2D,localYinW)]
#%make sure point is in the "box" defined by the line
if ((projectedPt2DinLocal[0]<0.051) and (projectedPt2DinLocal[0] > -0.001)
and (projectedPt2DinLocal[1]<0.001) and (projectedPt2DinLocal[1]>-0.001)):
projectedPcdLocal.append(projectedPt2DinLocal)
projectedPcd.append(pcd[i])
#Create a KDTree for searching
projectedPcdTree = spatial.KDTree(projectedPcdLocal)
###############Find the corresponding point on the surface to the line############
surfacePtsAll = []# %These are the surface pts that will be displaced.
#%part of the probe not in contact with the object...
#average 3 neighbors
NofN = 3
#rigidPointsFinal = [] ##we have a potential bug here for not using rigidPointsFinal....
for i in range(int(N)):
tmp =vo.mul(vo.sub(lineEnd0,lineStart0),s[i])#%the point on the line, projected
linePt = [vo.dot(tmp,localXinW),vo.dot(tmp,localYinW)]
d,Idx = projectedPcdTree.query(linePt[0:2],k=NofN)
#We might end up having duplicated pts...
#We should make sure that the discretization is not too fine..
#or should average a few neighbors
if d[0] < 0.002:
surfacePt = [0]*10
for j in range(NofN):
surfacePt = vo.add(surfacePt,projectedPcd[Idx[j]][0:10])
surfacePt = vo.div(surfacePt,NofN)
surfacePtsAll.append(surfacePt) #position in the global frame..
#rigidPointsFinal.append(linePts)
N = len(surfacePtsAll)
##### Preprocesss done
if not o3d:
vis.show()
time.sleep(15.0)
############# Go through a list of displacements
totalFinNList = []
#for queryD = -0.003:0.001:0.014
for queryD in queryDList:
lineStart = vo.sub(lineStart0,vo.mul(lineNormal,queryD))
lineEnd = vo.sub(lineEnd0,vo.mul(lineNormal,queryD))
lineCenter = vo.div(vo.add(lineStart,lineEnd),2)
torqueCenter = vo.add(lineCenter,vo.mul(lineNormal,0.09))
if vision_task_1:
vis.lock()
print(queryD)
x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0)
y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]]
z_axis = vo.cross(x_axis,y_axis)
z_axis = [z_axis[0],z_axis[1],z_axis[2]]
R = x_axis+y_axis+z_axis
t = vo.add(lineCenter,vo.mul(lineNormal,0.002))
print(R,t)
probe.setTransform(R,t)
vis.unlock()
time.sleep(dt)
else:
if not o3d:
vis.lock()
print(queryD)
x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0)
y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]]
z_axis = vo.cross(x_axis,y_axis)
z_axis = [z_axis[0],z_axis[1],z_axis[2]]
R = x_axis+y_axis+z_axis
t = vo.add(lineCenter,vo.mul(lineNormal,0.003))
probe.setTransform(R,t)
####calculate the nominal displacements
surfacePts = [] #These are the surface pts that will be displaced...
rigidPtsInContact = []
nominalD = [] #Column Vector..
for i in range(int(N)): ##we have a potential bug here for keep interating trhough N, since about, if d[0] < 0.002, the points is not added...
#weird no bug occured...
linePt = vo.add(vo.mul(vo.sub(lineEnd,lineStart),s[i]),lineStart)
surfacePt = surfacePtsAll[i][0:3]
normal = surfacePtsAll[i][6:9]
nominalDisp = -vo.dot(vo.sub(linePt,surfacePt),normal)
if nominalDisp > 0:
surfacePts.append(surfacePtsAll[i][0:10])#position in the global frame..
rigidPtsInContact.append(linePt)
nominalD.append(nominalDisp)
originalNominalD = deepcopy(nominalD)
NofSurfacePts = len(surfacePts)
if NofSurfacePts > 0:
negativeDisp = True
while negativeDisp:
NofSurfacePts = len(surfacePts)
K = np.zeros((NofSurfacePts,NofSurfacePts))
for i in range(NofSurfacePts):
for j in range(NofSurfacePts):
K[i][j] = invKernel(surfacePts[i][0:3],surfacePts[j][0:3],param)
#print K
#startTime = time.time()
actualD = np.dot(np.linalg.inv(K),nominalD)
#print('Time spent inverting matrix',time.time()- startTime)
#print nominalD,actualD
negativeIndex = actualD < 0
if np.sum(negativeIndex) > 0:
actualD = actualD.tolist()
surfacePts = [surfacePts[i] for i in range(len(surfacePts)) if actualD[i]>=0]
nominalD = [nominalD[i] for i in range(len(nominalD)) if actualD[i]>=0]
rigidPtsInContact = [rigidPtsInContact[i] for i in range(len(rigidPtsInContact)) if actualD[i]>=0]
else:
negativeDisp = False
#hsv's hue: 0.25 = 0.0002 displacement 1 == 10 displacement
#generate the entire displacement field
#surfacePts are the equlibrium pts that provide
entireDisplacedPcd = []
colorThreshold = 0.0005
colorUpperBound = 0.012
forceColorUpperBound = 0.004
colorRange = colorUpperBound - colorThreshold
greyColor = 0.38
xyz = []
rgb = []
xyzForce = []
rgbForce = []
maxDisp = 0
for pt,ptNormal in zip(equilibriumPcd.points,equilibriumPcd.normals):
shapeVector = []
for pt2 in surfacePts:
shapeVector.append(invKernel(pt[0:3],pt2[0:3],param))
nominalDisplacement = vo.dot(shapeVector,actualD)
if nominalDisplacement > maxDisp:
maxDisp = nominalDisplacement
color = colorsys.hsv_to_rgb(nominalDisplacement/colorRange*0.75+0.25,1,0.75)
color = [color[0],color[1],color[2]]
displacedDeformablePoint = vo.sub(pt[0:3],vo.mul(ptNormal,nominalDisplacement))
xyz.append(displacedDeformablePoint)
rgb.append(color)
counter = 0
for (p,c) in zip(xyz,equilibriumPcd.colors):#rgb):
klampt_pcd.setProperty(counter,'r',c[0])
klampt_pcd.setProperty(counter,'g',c[1])
klampt_pcd.setProperty(counter,'b',c[2])
klampt_pcd.setPoint(counter,p)
counter = counter + 1
klampt_pcd_object.geometry().setPointCloud(klampt_pcd)
if o3d:
open3dPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32))
open3dPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32))
open3dPcd.estimate_normals(search_param = open3d.geometry.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 30))
###open3dPcd,ind=statistical_outlier_removal(open3dPcd,nb_neighbors=10,std_ratio=2)
#open3d.visualization.draw_geometries([open3dPcd])#_box_bottom])
vis3.add_geometry(open3dPcd)
vis3.poll_events()
vis3.update_renderer()
if not o3d:
vis.unlock()
time.sleep(dt)
else:
pass
while vis.shown():
time.sleep(0.1)
return
def linearKernel(p1,p2,param):
r = vo.norm(vo.sub(p1,p2))
if r <= param:
return (param-r)/param
else:
return 0
def solve(self,robot=None,params=IKSolverParams()):
"""Globally solves the given problem. Returns the solution
configuration or None if failed."""
#set this to False if you want to run the local optimizer for each
#random restart.
postOptimize = True
t0 = time.time()
if len(self.objectives) == 0:
if self.costFunction is not None or self.feasibilityTest is not None:
raise NotImplementedError("Raw optimization without IK goals not done yet")
return None
if robot is None:
if not hasattr(self.objectives[0],'robot'):
print "The objectives passed to IKSolver should come from ik.objective() or have their 'robot' member manually set"
robot = self.objectives[0].robot
else:
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
if self.activeDofs is not None:
solver.setActiveDofs(self.activeDofs)
ikActiveDofs = self.activeDofs
if self.jointLimits is not None: solver.setJointLimits(*self.jointLimits)
qmin,qmax = solver.getJointLimits()
if self.activeDofs is None:
#need to distinguish between dofs that affect feasibility vs
ikActiveDofs = solver.getActiveDofs()
if self.costFunction is not None or self.feasibilityTest is not None:
activeDofs = [i for i in range(len(qmin)) if qmin[i] != qmax[i]]
nonIKDofs = [i for i in activeDofs if i not in ikActiveDofs]
ikToActive = [activeDofs.index(i) for i in ikActiveDofs]
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(activeDofs))
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(ikActiveDofs))
#sample random start point
if params.startRandom:
solver.sampleInitial()
if len(nonIKDofs)>0:
q = robot.getConfig()
for i in nonIKDofs:
q[i] = random.uniform(qmin[i],qmax[i])
robot.setConfig(q)
if params.localMethod is not None or params.globalMethod is not None:
#set up optProblem, an instance of optimize.Problem
optProblem = optimize.Problem()
Jactive = [[0.0]*len(activeDofs)]*len(solver.getResidual())
def ikConstraint(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if NEW_KLAMPT:
def ikConstraintJac(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getJacobian()
else:
#old version of Klamp't didn't compute the jacobian w.r.t. the active DOFS
def ikConstraintJac(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
Jikdofs = solver.getJacobian()
for i in ikActiveDofs:
for j in xrange(len(Jactive)):
Jactive[j][ikToActive[i]] = Jikdofs[j][i]
return Jactive
def costFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
return self.costFunction(q)
def feasFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
return self.feasibilityTest(q)
optProblem.addEquality(ikConstraint,ikConstraintJac)
if len(qmax) > 0:
optProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs])
if self.costFunction is None:
optProblem.setObjective(lambda x:0)
else:
optProblem.setObjective(costFunc)
if self.feasibilityTest is not None:
optProblem.setFeasibilityTest(feasFunc)
#optProblem is now ready to use
if self.softObjectives:
softOptProblem = optimize.Problem()
def costFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return vectorops.normSquared(solver.getResidual())*0.5
def costFuncGrad(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if len(qmax) > 0:
softOptProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs])
if self.feasibilityTest is not None:
softOptProblem.setFeasibilityTest(feasFunc)
softOptProblem.setObjective(costFunc,costFuncGrad)
#softOptProblem is now ready to use
if params.globalMethod is not None:
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
if self.softObjectives:
#globally optimize the soft objective function. If 0 objective value is obtained, use equality constrained
#optProblem. If 0 objective value is not obtained, constrain the residual norm-squared to be that value
(succ,res) = global_solve(softOptProblem,params,x0)
if not succ:
print "Global soft optimize returned failure"
return None
for d,v in zip(activeDofs,res):
q[d] = v
if self.costFunction is None:
#no cost function, just return
print "Global optimize succeeded! Cost",self.costFunction(q)
return q
x0 = res
#now modify the constraint of optProblem
robot.setConfig(q)
residual = solver.getResidual()
if max(abs(v) for v in residual) < params.tol:
#the constraint is satisfied, now just optimize cost
pass
else:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5*vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return [vectorops.normSquared(solver.getResidual())*0.5 - threshold]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality,inequalityGrad)
#do global optimization of the cost function and return
(succ,res) = global_solve(optProblem,params,x0)
if not succ:
print "Global optimize returned failure"
return None
for d,v in zip(activeDofs,res):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
print "Result from global optimize isn't feasible"
return None
if not softObjectives:
if max(abs(v) for v in solver.getResidual()) > params.tol:
print "Result from global optimize doesn't satisfy tolerance. Residual",vectorops.norm(solver.getResidual())
return None
#passed
print "Global optimize succeeded! Cost",self.costFunction(q)
return q
#DONT DO THIS... much faster to do newton solves first, then local optimize.
if not postOptimize and params.localMethod is not None:
if self.softObjectives:
raise RuntimeError("Direct local optimization of soft objectives is not done yet")
#random restart + local optimize
optSolver = optimize.LocalOptimizer(method=params.localMethod)
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
best = None
bestQuality = float('inf')
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
res = optSolver.solve(optProblem,params.numIters,params.tol)
if res[0]:
q = robot.getConfig()
for d,v in zip(activeDofs,res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
continue
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#random restart
solver.sampleInitial()
q = robot.getConfig()
if len(nonIKDofs)>0:
for i in nonIKDofs:
q[i] = random.uniform(qmin[i],qmax[i])
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
else:
#random-restart newton-raphson
solver.setMaxIters(params.numIters)
solver.setTolerance(params.tol)
best = None
bestQuality = float('inf')
if self.softObjectives:
#quality is a tuple
bestQuality = bestQuality,bestQuality
quality = None
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
t0 = time.time()
res = solver.solve()
if res or self.softObjectives:
q = robot.getConfig()
#check feasibility if desired
t0 = time.time()
if self.feasibilityTest is not None and not self.feasibilityTest(q):
if len(nonIKDofs) > 0:
u = float(restart+0.5)/params.numRestarts
q = robot.getConfig()
#perturbation sampling
for i in nonIKDofs:
delta = u*(qmax[i]-qmin[i])*0.5
q[i] = random.uniform(max(q[i]-delta,qmin[i]),min(q[i]+delta,qmax[i]))
robot.setConfig(q)
if not self.feasibilityTest(q):
solver.sampleInitial()
continue
else:
solver.sampleInitial()
continue
if self.softObjectives:
residual = solver.getResidual()
ikerr = max(abs(v) for v in residual)
if ikerr < params.tol:
ikerr = 0
else:
#minimize squared error
ikerr = vectorops.normSquared(residual)
if self.costFunction is None:
cost = 0
if ikerr == 0:
#feasible and no cost
return q
else:
cost = self.costFunction(q)
quality = ikerr,cost
else:
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#sample a new ik seed
solver.sampleInitial()
#post-optimize using local optimizer
if postOptimize and best is not None and params.localMethod is not None:
if self.softObjectives:
robot.setConfig(best)
residual = solver.getResidual()
if max(abs(v) for v in residual) > params.tol:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5*vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return [vectorops.normSquared(solver.getResidual())*0.5 - threshold]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality,inequalityGrad)
optSolver = optimize.LocalOptimizer(method=params.localMethod)
x0 = [best[d] for d in activeDofs]
optSolver.setSeed(x0)
res = optSolver.solve(optProblem,params.numIters,params.tol)
if res[0]:
q = robot.getConfig()
for d,v in zip(activeDofs,res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
pass
elif self.costFunction is None:
#feasible
best = q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
#print "Optimization improvement",bestQuality,"->",quality
best = q
bestQuality = quality
elif quality > bestQuality + 1e-2:
print "Got worse solution by local optimizing?",bestQuality,"->",quality
return best
def setGravity(self, g):
if len(g) == 3 and vectorops.norm(g) < 10.0:
self._gravity = g
def make_grasp_map(world, id):
"""Estimates a grasp quality and regression map for a sensor image.
Input:
world (WorldModel): contains a robot and a sensor
Output: a 4-channel numpy array of size (w,h,3) with
w x h the sensor dimensions and all values in the range [0,1].
The first channel encodes the quality.
The second encodes the grasp opening amount.
The third and fourth encode the orientation of the grasp
relative to the camera frame, as a heading and elevation.
Make
sure to note how you've transformed these quantities to
[0,1]! These will be needed for decoding.
"""
robot = world.robot(0)
sensor = robot.sensor(0)
sensor_xform = sensing.get_sensor_xform(sensor, robot)
w = int(sensor.getSetting("xres"))
h = int(sensor.getSetting("yres"))
#You'll be filling this image out
result = np.zeros((h, w, 4))
result[:, :, 3] = 0.5
#this shows how to go from a point in space to a pixel
# point = (0,0,0)
# projected = sensing.camera_project(sensor,robot,point)
# if projected is None:
# pass
# else:
# x,y,z = projected
# if x < 0 or x > w or y < 0 or y > h:
# pass
# else:
# result[int(y),int(x),0]=1
# result[50,10,0]=1
#load the gripper info and grasp database
source_gripper = robotiq_85
global gripper_robot, gripper_world, grasp_db
if gripper_robot is None:
gripper_world = WorldModel()
if not gripper_world.readFile(source_gripper.klampt_model):
raise ValueError("Unable to read gripper file " +
source_gripper.klampt_model)
gripper_robot = gripper_world.robot(0)
gripper_geom_open = source_gripper.get_geometry(gripper_robot,
source_gripper.open_config)
if grasp_db is None:
db = grasp_database.GraspDatabase(source_gripper)
if not db.load("../data/grasps/robotiq_85_more_sampled_grasp_db.json"):
raise RuntimeError("Can't load grasp database?")
grasp_db = db
for i in range(world.numRigidObjects()):
obj = world.rigidObject(i)
ycb_obj = obj.getName()
if ycb_obj not in grasp_db.object_to_grasps:
print("Can't find object", ycb_obj, "in database")
print("Database only contains objects", grasp_db.objects)
raise ValueError()
grasps = grasp_db.object_to_grasps[ycb_obj]
gripper_tip = vectorops.madd(source_gripper.center,
source_gripper.primary_axis,
source_gripper.finger_length)
num_grasps_valid = 0
for gindex, g in enumerate(grasps):
#TODO: problem 1B: do collision testing of gripper_geom
found_approach = False
if (id, ycb_obj, gindex) in grasps_feasible:
found_approach = grasps_feasible[(id, ycb_obj, gindex)]
else:
#this is the Geometry3D corresponding to the robot at the opening configuration
gripper_geom = source_gripper.get_geometry(
gripper_robot, g.finger_config)
local_pt, world_pt = g.ik_constraint.getPosition()
local_axis, world_axis = g.ik_constraint.getRotationAxis()
#TODO: put your code here
R = so3.vector_rotation(source_gripper.primary_axis,
world_axis)
t = vectorops.sub(world_pt, source_gripper.center)
gripper_geom_open.setCurrentTransform(R, t)
non_collision = True
for i in range(world.numRigidObjects()):
if i == gripper_robot.getID():
continue
if gripper_geom_open.collides(
world.rigidObject(i).geometry()):
non_collision = False
break
found_approach = True if non_collision else False
if found_approach:
gripper_geom.setCurrentTransform(R, t)
for i in range(world.numRigidObjects()):
if i == gripper_robot.getID() or i == obj.getID():
continue
if gripper_geom.collides(
world.rigidObject(i).geometry()):
found_approach = False
break
grasps_feasible[(id, ycb_obj, gindex)] = found_approach
if not found_approach:
continue
num_grasps_valid += 1
Tfixed = g.ik_constraint.closestMatch(*se3.identity())
Tworld = se3.mul(obj.getTransform(), Tfixed)
gworld = se3.apply(Tworld, gripper_tip)
projected = sensing.camera_project(sensor, robot, gworld)
if projected is not None:
x, y, z = projected
x = int(x)
y = int(y)
if x < 0 or x >= w or y < 0 or y >= h:
continue
gripper_opening_axis = so3.apply(Tworld[0],
source_gripper.secondary_axis)
gripper_opening_axis_cam = so3.apply(so3.inv(sensor_xform[0]),
gripper_opening_axis)
if gripper_opening_axis_cam[1] < 0:
gripper_opening_axis_cam = vectorops.mul(
gripper_opening_axis_cam, -1)
gripper_axis_heading = math.atan2(gripper_opening_axis_cam[1],
gripper_opening_axis_cam[0])
xy = vectorops.norm(gripper_opening_axis_cam[:2])
if xy < 1e-7:
gripper_axis_elevation = math.pi * 0.5
else:
gripper_axis_elevation = math.atan(
gripper_opening_axis_cam[2] / xy)
score0 = math.exp(-g.score)
window = 10
std = 5
for u in range(-window, window + 1):
if y + u < 0 or y + u >= h: continue
for v in range(-window, window + 1):
if x + v < 0 or x + v >= w: continue
score = score0 * math.exp(-(u**2 + v**2) /
(2 * std**2))
if score > result[y + u, x + v, 0]:
result[y + u, x + v, :] = [
score,
source_gripper.config_to_opening(
g.finger_config),
gripper_axis_heading / math.pi,
gripper_axis_elevation / math.pi + 0.5
]
print("{}/{} grasps valid for object {}".format(
num_grasps_valid, len(grasps), ycb_obj))
return result
def invKernel(p1,p2,param):
r = vo.norm(vo.sub(p1,p2))
return param/(param+r)
