def draw_GL_frame(T, axis_length=0.1, color=None):
"""Draws the rigid transform T as a set of axis-oriented lines
of length axis_length."""
R, t = T
if len(R) != 9 or len(t) != 3:
print "Incorrect sizes", len(R), len(t)
raise RuntimeError("")
#draw transform widget
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(1.5)
if color is None:
glColor3f(1, 0, 0)
else:
glColor3f(*color)
glBegin(GL_LINES)
glVertex3fv(t)
glVertex3fv(vectorops.madd(t, R[0:3], axis_length))
glEnd()
if color is None:
glColor3f(0, 1, 0)
glBegin(GL_LINES)
glVertex3fv(t)
glVertex3fv(vectorops.madd(t, R[3:6], axis_length))
glEnd()
if color is None:
glColor3f(0, 0, 1)
glBegin(GL_LINES)
glVertex3fv(t)
glVertex3fv(vectorops.madd(t, R[6:9], axis_length))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
"""
def predict_next_location(self, x, v, t, gravity):
# update estimated x value
x_temp = vectorops.sub(vectorops.madd(x, v, t),
[0, 0, 0.95 * gravity * t * t])
# if x_temp[2] is less than 0, then extrapolate exact time where t would've hit 0.03
if (x_temp[2] < 0.03):
# extrapolate exact time where t would've hit
t = t * ((x[2] - 0.03) / (x[2] - x_temp[2]))
# update x position
x = vectorops.sub(vectorops.madd(x, v, t),
[0, 0, 0.95 * gravity * t * t])
# set the z coordinate to 0 (to be sure)
x[2] = 0.03
# update velocity value
val = 0.7
v[0] = v[0] * val
v[1] = v[1] * val
v[2] = (v[2] * -1 * val) - .95 * gravity * t
else:
x = x_temp
v = vectorops.sub(v,
[0, 0, 0.95 * gravity * t
]) # t is updated based on above computations
return (x, v)
def start(self, args):
if 'limb' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'velocity' not in args:
self.onInvalidParameters("invalid argument list")
return False
limb = args['limb']
dangles = args['velocity']
maxTime = args.get('maxTime', 1.0)
safe = args.get('safe', False)
#deadband
for i, v in enumerate(dangles):
if abs(v) < 1e-4: dangles[i] = 0
if safe:
self.onInvalidParameters("Safe mode not supported yet")
return False
if limb == 'left' or limb == 0:
if len(dangles) != 7:
self.onInvalidParameters(
"invalid 'value' argument, not size 7")
return False
angles = vectorops.madd(robot.left_limb.commandedPosition(),
dangles, maxTime)
robot.left_mq.setLinear(maxTime, angles)
self.mqs = [robot.left_mq]
elif limb == 'right' or limb == 1:
if len(dangles) != 7:
self.onInvalidParameters(
"invalid 'value' argument, not size 7")
return False
angles = vectorops.madd(robot.right_limb.commandedPosition(),
dangles, maxTime)
robot.right_mq.setLinear(maxTime, angles)
self.mqs = [robot.right_mq]
elif limb == 'both' or limb == 2:
if len(dangles) != 14:
self.onInvalidParameters(
"invalid 'value' argument, not size 14")
return False
langles = vectorops.madd(robot.left_limb.commandedPosition(),
dangles[:7], maxTime)
rangles = vectorops.madd(robot.right_limb.commandedPosition(),
dangles[7:], maxTime)
robot.left_mq.sendLinear(maxTime, langles)
robot.right_mq.sendLinear(maxTime, rangles)
self.mqs = [robot.left_mq, robot.right_mq]
else:
self.onInvalidParameters("invalid 'limb' setting: %s" % (limb, ))
return False
#a zero velocity command is just a stop
if max(abs(v) for v in dangles) == 0:
self._status = 'done'
else:
self._status = 'ok'
return True
def compute_initial_final(world, robot):
"""
Initial point is fixed and given, target is set by
1. rotate q0 a random angle between -pi/4, 3*pi/4
2. random perturb ee position by 0.3 m
If IK fails, simply abandon it.
Exit when 10 plans are generated.
"""
end_effector = robot.link(7)
space = makeSpace(world, robot)
# we want to the end effector to move from start to goal
start = [-0.2, -0.50, 1.4]
# solve IK for the start point
unit_vec1 = [0.0, -0.1, 0.0]
null = [0.0, 0.0, 0.0]
unit_vec2 = [0.0, 0., -0.1]
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[start, vo.madd(start, unit_vec2, 1)])
collision_check = lambda: (space.selfCollision(robot.getConfig(
))) == False and (space.envCollision(robot.getConfig()) == False)
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for start point failed!")
qstart = robot.getConfig()
print('Feasible ik start ', robot.getConfig())
# then IK for goal
qtmp = qstart[:]
qtmp[1] += np.random.random(
) * np.pi + np.pi / 2 # rotation angle, pi/2 to 3pi/2
robot.setConfig(qtmp)
eepos = np.array(end_effector.getWorldPosition(null))
rand_pos = eepos.copy()
rand_pos[:2] += np.random.random(2) * 0.3 - 0.15
rand_pos[2] += (np.random.random() * 0.8 - 0.4
) # this might be too large, we will see
goal = rand_pos.tolist()
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[goal, vo.madd(goal, unit_vec2, 1)])
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for goal point failed!")
print("Feasible ik goal ", robot.getConfig())
qgoal = robot.getConfig()
return np.array(qstart), np.array(qgoal)
def ikSolver(self, robot, obj_pt, obj_axis):
"""Returns an IK solver that places the hand center at obj_pt with
the palm oriented along obj_axis"""
q = robot.getConfig()
obj = IKObjective()
obj.setFixedPoints(self.link,
[self.localPosition1, self.localPosition2], [
vectorops.madd(obj_pt, obj_axis, -0.03),
vectorops.madd(obj_pt, obj_axis, 0.03)
])
solver = IKSolver(robot)
solver.add(obj)
solver.setActiveDofs(self.armIndices)
return solver
def advance(self, **inputs):
try:
dt = inputs["dt"]
v = inputs[self.name]
except KeyError:
raise ValueError("Input needs to have value %s and timestep 'dt'" %
(self.name, ))
if len(v) == 0: return None
if self.integral is None:
self.integral = vectorops.mul(v, dt)
else:
self.integral = vectorops.madd(self.integral, v, dt)
result = vectorops.madd(self.integral, v, -0.5 * dt)
return {'I' + self.name: result}
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.
"""
if self.objectEstimates:
for o in self.objectEstimates.objects:
#draw a point
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])
#draw an arc
trace = []
x = [o.x[0],o.x[1],o.x[2]]
v = [o.x[3],o.x[4],o.x[5]]
if event=='C': gravity = 0
else: gravity = 9.8
for i in range(20):
t = i*0.05
trace.append(vectorops.sub(vectorops.madd(x,v,t),[0,0,0.5*gravity*t*t]))
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 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 add_coordinate_transform(ax,R,t,size=1.0,*args,**kwargs):
"""Draws a coordinate transform on the plot ax"""
axes = so3.matrix(so3.transpose(R))
colors = ['r','g','b']
for (v,c) in zip(axes,colors):
a = Arrow3D(t, vectorops.madd(t,v,size), lw=1, color=c, *args, **kwargs)
ax.add_artist(a)
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 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 updateVis(objectEstimates):
"""This gets called by update()
TODO: You may consider visually debugging some of your code here, along with initVis().
The current code draws gravity-inflenced arcs leading from all the
object position / velocity estimates from your state estimator.
"""
gravity = GRAVITY
if objectEstimates:
for o in objectEstimates.objects:
#draw a point
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])
#draw an arc
trace = []
x = [o.x[0], o.x[1], o.x[2]]
v = [o.x[3], o.x[4], o.x[5]]
for i in range(20):
t = i * 0.05
trace.append(
vectorops.sub(vectorops.madd(x, v, t),
[0, 0, 0.5 * gravity * t * t]))
if trace[-1][2] < 0: break
kviz.remove("object_trace" + str(o.name))
kviz.add_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 callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
def add_to_vis(self,name,color=(1,0,0,1)):
finger_radius = 0.02
if self.finger_width == None:
w = 0.05
else:
w = self.finger_width*0.5+finger_radius
a = vectorops.madd(self.center,self.axis,w)
b = vectorops.madd(self.center,self.axis,-w)
vis.add(name,Trajectory(milestones=[a,b]),color=color)
if self.approach is not None:
vis.add(name+"_approach",Trajectory(milestones=[self.center,vectorops.madd(self.center,self.approach,0.05)]),color=(1,0.5,0,1))
normallen = 0.05
if self.contact1 is not None:
vis.add(name+"cp1",self.contact1.x,color=(1,1,0,1),size=0.01)
vis.add(name+"cp1_normal",Trajectory(milestones=[self.contact1.x,vectorops.madd(self.contact1.x,self.contact1.n,normallen)]),color=(1,1,0,1))
if self.contact2 is not None:
vis.add(name+"cp2",self.contact2.x,color=(1,1,0,1),size=0.01)
vis.add(name+"cp2_normal",Trajectory(milestones=[self.contact2.x,vectorops.madd(self.contact2.x,self.contact2.n,normallen)]),color=(1,1,0,1))
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(), self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(), self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(), self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def update_robot_viz(x, y, z):
vis.clear()
vis.add("robot", display_robot)
for i, o in enumerate(objs):
odisplay = wdisplay.rigidObject(i)
vis.add("obj_{}".format(i), odisplay)
try:
display_robot.setConfig(x)
for i, yi in enumerate(y):
vis.add("pt_{}".format(i),
yi[1],
color=(1, 0, 0, 1),
hide_label=True)
f_normal = z[i * 7 + 6]
f_friction = z[i * 7:i * 7 + 6]
n = np.array(normal(grasping_problem.xyz_eq.env_geom, yi[1]))
f = n * f_normal
n1 = so3.canonical(n)[3:6]
n2 = so3.canonical(n)[6:9]
for j in range(6):
n_tmp = (math.cos(
(math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2))
f += n_tmp * f_friction[j]
# f is the force pointing inward
vis.add(
"n_{}".format(i),
Trajectory([0, 1],
[yi[1], vectorops.madd(yi[1], n, 0.05)]),
color=(1, 1, 0, 1),
hide_label=True)
vis.add(
"f_{}".format(i),
Trajectory(
[0, 1],
[yi[1], vectorops.madd(yi[1], f.tolist(), -1.0)]),
color=(1, 0.5, 0, 1),
hide_label=True)
finally:
pass
time.sleep(0.01)
def next_state(self,x,u):
assert len(x) == 3
assert len(u) == 2
pos = [x[0],x[1]]
fwd = [math.cos(x[2]),math.sin(x[2])]
right = [-fwd[1],fwd[0]]
phi = u[1]
d = u[0]
if abs(phi)<1e-8:
newpos = vectorops.madd(pos,fwd,d)
return newpos + [x[2]]
else:
#rotate about a center of rotation, with radius 1/phi
cor = vectorops.madd(pos,right,1.0/phi)
sign = cmp(d*phi,0)
d = abs(d)
phi = abs(phi)
theta=0
thetaMax=d*phi
newpos = vectorops.add(so2.apply(sign*thetaMax,vectorops.sub(pos,cor)),cor)
return newpos + [so2.normalize(x[2]+sign*thetaMax)]
def constantVServo(controller, servoTime, target, dt):
currentTime = 0.0
goalConfig = deepcopy(target)
currentConfig = controller.getConfig()
difference = vectorops.sub(goalConfig, currentConfig)
while currentTime < servoTime:
setConfig = vectorops.madd(currentConfig, difference,
currentTime / servoTime)
controller.setConfig(setConfig)
time.sleep(dt)
currentTime = currentTime + dt
print currentTime
def advance(self, **inputs):
val = inputs[self.argname]
if hasattr(val, '__iter__'):
res = vectorops.mul(val, self.b[0])
assert len(self.history) + 1 <= len(self.b)
for i, v in enumerate(self.history):
res = vectorops.madd(res, v, self.b[i + 1])
if len(self.history) + 1 < len(self.b):
res = vectorops.madd(res, self.history[-1],
sum(self.b[len(self.history) + 1:]))
else:
res = val * self.b[0]
assert len(self.history) + 1 <= len(self.b)
for i, v in enumerate(self.history):
res += v * self.b[i + 1]
if len(self.history) + 1 < len(self.b):
res += self.history[-1] * sum(self.b[len(self.history) + 1:])
#advance history
self.history.appendleft(val)
while len(self.history) >= len(self.b):
self.history.pop()
return res
def ik_solve(target_position, first_time=False):
left_ee_link = robot.link(13)
left_active_dofs = [7, 8, 9, 10, 11, 12]
ee_local_pos = [0, 0, 0]
h = 0.1
local = [ee_local_pos, vectorops.madd(ee_local_pos, (1, 0, 0), -h)]
world = [target_position, vectorops.madd(target_position, (0, 0, 1), h)]
goal = ik.objective(left_ee_link, local=local, world=world)
if first_time:
solved = ik.solve_global(goal,
activeDofs=left_active_dofs,
startRandom=True)
else:
solved = ik.solve(goal, activeDofs=left_active_dofs)
if solved:
print("solve success")
return True
else:
print("Solve unsuccessful")
return False
def check_collision_linear(robot, collider, start, end, N):
initialConfig = robot.getConfig()
diff = vectorops.sub(end, start)
flag = False
for i in range(N):
config = (vectorops.madd(start, diff, float(i) / float(N)))
if check_collision_single(robot, collider, config):
flag = True
robot.setConfig(config)
break
if check_collision_single(robot, collider, end):
flag = True
robot.setConfig(initialConfig)
return flag
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0,1,1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1,0.5,0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(),self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(),self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][1],fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(),self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][2],fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
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 control_loop(self):
#Calculate the desired velocity for each robot by adding up all
#commands
rvels = [[0]*self.world.robot(r).numLinks() for r in range(self.world.numRobots())]
for (c,(r,v)) in self.current_velocities.iteritems():
rvels[r] = vectorops.add(rvels[r],v)
#print rvels
#send to the robot(s)
for r in range(self.world.numRobots()):
robotController = self.sim.controller(r)
qdes = robotController.getCommandedConfig()
qdes = vectorops.madd(qdes,rvels[r],self.dt)
#clamp to joint limits
(qmin,qmax) = self.world.robot(r).getJointLimits()
for i in xrange(len(qdes)):
qdes[i] = min(qmax[i],max(qdes[i],qmin[i]))
robotController.setPIDCommand(qdes,rvels[r])
return
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 control_loop(self):
sim = self.sim
world = self.world
controller = sim.controller(0)
robotModel = world.robot(0)
t = self.ttotal
#Problem 1: tune the values in this line
if problem == "1a" or problem == "1b":
kP = 1
kI = 1
kD = 1
controller.setPIDGains([kP], [kI], [kD])
#Problem 2: use setTorque to implement a control loop with feedforward
#torques
if problem == "2a" or problem == "2b":
qcur = controller.getSensedConfig()
dqcur = controller.getSensedVelocity()
qdes = [0]
dqdes = [0]
robotModel.setConfig(qcur)
robotModel.setVelocity(dqcur)
while qcur[0] - qdes[0] > math.pi:
qcur[0] -= 2 * math.pi
while qcur[0] - qdes[0] < -math.pi:
qcur[0] += 2 * math.pi
ddq = vectorops.mul(vectorops.sub(qdes, qcur), 100.0)
ddq = vectorops.madd(ddq, vectorops.sub(dqdes, dqcur), 20.0)
#TODO: solve the dynamics equation to fill this in
T = [3]
controller.setTorque(T)
#Problem 3: drive the robot so its end effector goes
#smoothly toward the target point
if problem == "3":
target = [1.0, 0.0, 0.0]
qdes = [0.7, -2.3]
dqdes = [0, 0]
controller.setPIDCommand(qdes, dqdes)
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 interpolate_linear(a,b,u):
"""Interpolates linearly in cartesian space between a and b."""
return vectorops.madd(a,vectorops.sub(b,a),u)
def makeHapticCartesianPoseMsg(self):
deviceState = self.haptic_client.deviceState
if len(deviceState)==0:
print "No device state"
return None
transforms = [self.serviceThread.eeGetter_left.get(),self.serviceThread.eeGetter_right.get()]
update = False
commanding_arm=[0,0]
for i,dstate in enumerate(deviceState):
if dstate.buttonDown[0]:
commanding_arm[i]=1
if self.startTransforms[i] == None:
self.startTransforms[i] = transforms[i]
#RESET THE HAPTIC FORCE FEEDBACK CENTER
#self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=dstate.position,device=i)
#UPDATE THE HAPTIC FORCE FEEDBACK CENTER FROM ROBOT EE POSITION
#need to invert world coordinates to widget coordinates to haptic device coordinates
#widget and world translations are the same
dx = vectorops.sub(transforms[i][1],self.startTransforms[i][1]);
dxwidget = vectorops.div(dx,hapticArmTranslationScaling)
R,device_center = widgetToDeviceTransform(so3.identity(),vectorops.add(dxwidget,dstate.widgetInitialTransform[0][1]))
#print "Device position error",vectorops.sub(dstate.position,device_center)
if vectorops.distance(dstate.position,device_center) > 0.03:
c = vectorops.madd(device_center,vectorops.unit(vectorops.sub(dstate.position,device_center)),0.025)
self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=c,device=i)
else:
#deactivate
self.serviceThread.hapticupdater.activateSpring(kP=0,kD=0,device=i)
if dstate.newupdate:
update = True
else:
if self.startTransforms[i] != None:
self.serviceThread.hapticupdater.deactivateHapticFeedback(device=i)
self.startTransforms[i] = None
dstate.newupdate = False
if update:
msg = {}
msg['type'] = 'CartesianPose'
T1 = self.getDesiredCartesianPose('left',0)
R1,t1=T1
T2 = self.getDesiredCartesianPose('right',1)
R2,t2=T2
transforms = [(R1,t1),(R2,t2)]
if commanding_arm[0] and commanding_arm[1]:
msg['limb'] = 'both'
msg['position'] = t1+t2
msg['rotation'] = R1+R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
elif commanding_arm[0] ==0:
msg['limb'] = 'right'
msg['position'] = t2
msg['rotation'] = R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
msg['limb'] = 'left'
msg['position'] = t1
msg['rotation'] = R1
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
return None
def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
def interpolate(a, b, u):
"""Interpolates linearly between a and b"""
return vectorops.madd(a, vectorops.sub(b, a), u)
def moveForceSpring(self, x, y):
self.sim.updateWorld()
(s, d) = self.view.click_ray(x, y)
u = vectorops.dot(vectorops.sub(self.forceAnchorPoint, s), d)
self.forceAnchorPoint = vectorops.madd(s, d, u)
def solve(self, q,dq,dt): """Takes sensed q,dq, timestep dt and returns qdes and dqdes in joint space. """ for task in self.taskList: task.updateState(q,dq,dt) # priority 1 if not hasattr(self,'timingStats'): self.timingStats = defaultdict(int) self.timingStats['count'] += 1 t1 = time.time() J1 = self.getStackedJacobian(q,dq,1) v1 = self.getStackedVelocity(q,dq,dt,1) (A,b) = self.getMotionModel(q,dq,dt) if self.activeDofs != None: A = select_cols(A,self.activeDofs) if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A): A = A.tocsr() t2 = time.time() self.timingStats['get jac/vel p1'] += t2-t1 J2 = self.getStackedJacobian(q,dq,2) if J2 is not None: V2 = self.getStackedVelocity(q,dq,dt,2) t3 = time.time() self.timingStats['get jac/vel p2'] += t3-t2 #compute velocity limits vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax,-1.0) amax = self.robot.getAccelerationLimits() vref = dq if self.ulast == None else self.ulast for i,(v,vm,am) in enumerate(zip(vref,vmin,amax)): if v-dt*am > vm: vmin[i] = v-dt*am elif v < vm: #accelerate! vmin[i] = min(vm,v+dt*am) for i,(v,vm,am) in enumerate(zip(vref,vmax,amax)): if v-dt*am < vm: vmax[i] = v+dt*am elif v > vm: #decelerate! vmax[i] = max(vm,v-dt*am) for i,(l,u) in enumerate(zip(vmin,vmax)): assert l <= u if l > 0 or u < 0: print "Moving link:",self.robot.link(i).getName(),"speed",vref[i] #print zip(vmin,vmax) Aumin = np.array(vmin) - b Aumax = np.array(vmax) - b #print zip(Aumin.tolist(),Aumax.tolist()) J1A = J1.dot(A) J1b = J1.dot(b) if J2 == None: #just solve constrained least squares #J1*(A*u+b) = v1 #vmin < A*u + b < vmax u1 = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax)[0] u2 = [0.0]*len(u1) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 else: #solve equality constrained least squares #dq = A*u + b #J1*dq = v1 #J1*A*u + J1*b = v1 #least squares solve for u1: #J1*A*u1 = v1 - J1*b #vmin < A*u1 + b < vmax #need u to satisfy #Aact*u = bact #we know that u1 satisfies Aact*u = bact #let u = u1+u2 #=> u2 = (I - Aact^+ Aact) z = N*z #least squares solve for z: #J2*A*(u1+u2+b) = v2 #J2*A*N z = v2 - J2*(A*u1+b) (u1, active, activeRhs) = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax) Aact = sparse.vstack([J1A]+[A[crow,:] for crow in active]).todense() #bact = np.hstack((v1-J1b,activeRhs)) J1Ainv = np.linalg.pinv(Aact) dq1 = A.dot(u1)+b if len(active)>0: print "Priority 1 active constraints:" for a in active: print self.robot.link(a).getName(),vmin[a],dq1[a],vmax[a] r1 = J1.dot(dq1)-v1 print "Op space controller solve" print " Residual 1",np.linalg.norm(r1) # priority 2 N = np.eye(len(dq)) - np.dot(J1Ainv, Aact) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 u2 = [0.0]*len(u1) #print " Initial priority 2 task error",np.linalg.norm(V2-J2.dot(dq1)) J2A = J2.dot(A) J2AN = J2A.dot(N) AN = sparse.csr_matrix(np.dot(A.todense(),N)) #Note: N destroys sparsity V2_m_resid = np.ravel(V2 - J2.dot(dq1)) (z,active,activeRhs) = constrained_lsqr(J2AN,V2_m_resid,AN,vmin-dq1,vmax-dq1) t5 = time.time() self.timingStats['ls jac p2'] += t5-t4 u2 = np.ravel(np.dot(N, z)) #debug, should be close to zero #print " Nullspace projection error:",np.linalg.norm(J1A.dot(u2)) #this is the error in the nullspace of the first priority tasks dq2 = A.dot(u2) + dq1 #debug, should be equal to residual 2 printout above print " Residual 2",np.linalg.norm(J2.dot(dq2)-V2) #debug should be close to zero #print " Residual 2 in priority 1 frame",np.linalg.norm(J1.dot(dq2)-v1) if len(active)>0: print "Priority 2 active constraints:" for a in active: print self.robot.link(a).getName(),vmin[a],dq2[a],vmax[a] #compose the velocities together u = np.ravel((u1 + u2)) dqpred = A.dot(u)+b print " Residual 1 final",np.linalg.norm(np.ravel(J1.dot(dqpred))-v1) if J2 != None: print " Residual 2 final",np.linalg.norm(np.ravel(J2.dot(dqpred))-V2) u = u.tolist() #if self.activeDofs != None: # print "dqdes:",[self.dqdes[v] for v in self.activeDofs] self.qdes = vectorops.madd(q, u, dt) self.ulast = u t6 = time.time() self.timingStats['total']+=t6-t1 if self.timingStats['count']%10==0: n=self.timingStats['count'] print "OpSpace times (ms): vel/jac 1 %.2f inv 1 %.2f vel/jac 2 %.2f inv 2 %.2f total %.2f"%(self.timingStats['get jac/vel p1']/n*1000,self.timingStats['pinv jac p1']/n*1000,self.timingStats['get jac/vel p2']/n*1000,self.timingStats['ls jac p2']/n*1000,self.timingStats['total']/n*1000) return (self.qdes,u)