def scaleToBounds(val, bmin, bmax, origin=None):
"""Cap the ray starting origin in the direction val against bounding box limits.
"""
if origin != None:
if isinstance(bmin, np.ndarray):
bmin -= origin
else:
bmin = vectorops.sub(bmin, origin)
if isinstance(bmax, np.ndarray):
bmax -= origin
else:
bmax = vectorops.sub(bmax, origin)
return scaleToBounds(val, bmin, bmax)
umax = 1.0
for vali, ai, bi in zip(val, bmin, bmax):
assert bi >= ai
if vali < ai:
umax = ai / vali
if vali > bi:
umax = bi / vali
if umax == 1.0: return val
assert umax >= 0
print "Scaling to velocity maximum, factor", umax
if isinstance(val, np.ndarray):
return val * umax
else:
return vectorops.mul(val, umax)
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 segment_segment_intersection(seg1, seg2, umin=0, umax=1, vmin=0, vmax=1):
"""Given 2 2D segments (a1,b1) and (a2,b2) returns whether the portion of
seg1 in the range [umin,umax] overlaps seg2 in the range [vmin,vmax].
Returns the point of intersection or None if no intersection exists
"""
a1, b1 = seg1
a2, b2 = seg2
assert len(a1) == 2
assert len(b1) == 2
assert len(a2) == 2
assert len(b2) == 2
d = vectorops.sub(b1, a1)
a, b = vectorops.sub(a2, a1), vectorops.sub(b2, a1)
#now it's centered at a1
#u*d = a + v*(b-a)
e = vectorops.sub(b2, a2)
A = np.array([d, vectorops.mul(e, -1)]).T
try:
uv = np.dot(np.linalg.inv(A), a)
except np.linalg.linalg.LinAlgError:
return None
u, v = uv
if u < umin or u > umax:
return None
if v < umin or v > umax:
return None
return vectorops.interpolate(a1, b1, u)
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 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 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 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 df_dz(self, x, y, z):
n = len(y)
m = len(z) // n
result = np.zeros((6, len(z)))
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
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)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T
result[0:3, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Normal_normal).reshape((3, 1))
result[3:6,
i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T
result[3:6, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Cross_Normal).reshape((3, 1))
return result
def scaleToBounds( val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin,np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin,origin) if isinstance(bmax,np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax,origin) return scaleToBounds(val,bmin,bmax) umax = 1.0 for vali,ai,bi in zip(val,bmin,bmax): assert bi >= ai if vali < ai: umax = ai/vali if vali > bi: umax = bi/vali if umax==1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor",umax if isinstance(val,np.ndarray): return val*umax else: return vectorops.mul(val,umax)
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 opt_error_fun(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
Roct = so3.from_rpy(est_input[0:3])
toct = est_input[3:6]
Rn = so3.from_rpy(est_input[6:9])
tn = est_input[9:12]
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
fun_list = np.append(
fun_list,
np.multiply(
so3.error(so3.inv(Tneedle2oct_icp[i][0]),
so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]),
Rn)), 1))
fun_list = np.append(
fun_list,
np.multiply(
vectorops.sub(
vectorops.sub([0., 0., 0.],
so3.apply(so3.inv(Tneedle2oct_icp[i][0]),
Tneedle2oct_icp[i][1])),
so3.apply(
so3.inv(Roct),
vectorops.add(vectorops.sub(Tlink_set[i][1], toct),
so3.apply(Tlink_set[i][0], tn)))), 1000))
return fun_list
def segment_point_distance(a, b, x):
"""Returns (distance, parameter) pair between segment (a,b) and point x"""
d = vectorops.sub(b, a)
u = vectorops.dot(vectorops.sub(x, a), d) / vectorops.dot(d, d)
u = min(1.0, max(u, 0.0))
d = vectorops.distance(x, vectorops.interpolate(a, b, u))
return (d, u)
def substep(self, dt):
twist = self.twist
#compute the angular velocity of the shell in the motor frame
motorBody = self.sim.body(self.robot.link(5))
shellBody = self.sim.body(self.robot.link(8))
motorTwist = motorBody.getVelocity()
shellTwist = shellBody.getVelocity()
motorXform = motorBody.getTransform()
shellXform = shellBody.getTransform()
shellRelativeAvel = so3.apply(
so3.inv(motorXform[0]), vectorops.sub(shellTwist[0],
motorTwist[0]))
#print "Relative angular vel",shellRelativeAvel
desiredTurnSpeed = twist[2] * self.velocityLimits[0]
desiredDriveSpeed = 0
if twist[0] == 0 or twist[0] * self.motorSpeeds[1] < 0: #stop
desiredDriveSpeed = 0
else:
desiredDriveSpeed = self.motorSpeeds[
1] + twist[0] * self.accelLimits[1] * self.dt
#print "Turn des",desiredTurnSpeed, "drive des",desiredDriveSpeed
#clamp speeds to limits
desiredTurnSpeed = max(-self.velocityLimits[0],
min(desiredTurnSpeed, self.velocityLimits[0]))
desiredDriveSpeed = max(-self.velocityLimits[1],
min(desiredDriveSpeed, self.velocityLimits[1]))
terr = desiredTurnSpeed - self.motorSpeeds[0]
derr = desiredDriveSpeed - self.motorSpeeds[1]
#clamp desired accelerations to limits
terr = max(-self.accelLimits[0] * self.dt,
min(terr, self.accelLimits[0] * self.dt))
derr = max(-self.accelLimits[1] * self.dt,
min(derr, self.accelLimits[1] * self.dt))
self.motorSpeeds[0] += terr
self.motorSpeeds[1] += derr
#apply locked velocity control to bring shell up to relative speed
#this is the desired angular velocity of the shell in the motor
#coordinates
desiredShellAvel = [self.motorSpeeds[1], 0, self.motorSpeeds[0]]
#print "Desired angular vel",desiredShellAvel
relativeVelError = vectorops.sub(desiredShellAvel, shellRelativeAvel)
wrenchlocal = vectorops.mul(relativeVelError, self.velocityLockGain)
#local wrench is k*(wdes-wrel)
wrench = so3.apply(motorXform[0], wrenchlocal)
#print "Wrench to shell",wrench
motorBody.applyWrench([0, 0, 0], vectorops.mul(wrench, -1.0))
shellBody.applyWrench([0, 0, 0], wrench)
#disable PID controllers
self.controller.setTorque([0, 0, 0])
#apply rolling friction forces
shellBody.applyWrench([0, 0, 0],
vectorops.mul(shellTwist[0],
-self.rollingFrictionCoeff))
return
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 config_to_opening(self,qfinger):
"""Estimates far qfinger is from closed_config to open_config.
Only meaningful if qfinger is close to being along the straight
C-space line between the two.
"""
if self.closed_config is None or self.open_config is None:
raise ValueError("Can't estimate opening width to")
assert len(qfinger) == len(self.closed_config)
b = vectorops.sub(qfinger,self.closed_config)
a = vectorops.sub(self.open_config,self.closed_config)
return min(1,max(0,vectorops.dot(a,b)/vectorops.normSquared(a)))
def calculate_workspace_axis(robot, obstacles, end_effector, point_local,
axis_local, axis_world):
global lower_corner, upper_corner
resolution = (15, 15, 15)
cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner),
resolution)
invcellsize = vectorops.div(resolution,
vectorops.sub(upper_corner, lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
return reachable
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 bbox(bmin,
bmax,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a box from bounds [bmin,bmax].
Args:
bmin (list of 3 floats): the lower corner of the box
center (list of 3 floats): the upper corner of the box
R,t (se3 transform, optional): if given, the box's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the box will be a
RigidObjectModel or TerrainModel will be created in this world
name (str, optional): If world is given, this is the name of the object.
Default is 'box'.
mass (float, optional): If world is given and this is inf, then a
TerrainModel will be created. Otherwise, a RigidObjectModel
will be created with automatically determined inertia.
type (str, optional): the geometry type. Defaults to 'TriangleMesh',
but also 'GeometricPrimitive' and 'VolumeGrid' are accepted.
Returns:
Geometry3D, RigidObjectModel, or TerrainModel: A representation
of the box. If a world is given, then either a RigidObjectModel
or TerrainModel is added to the world and returned.
"""
w, d, h = vectorops.sub(bmax, bmin)
center = vectorops.interpolate(bmin, bmax, 0.5)
return box(w, d, h, center, R, t, world, name, mass, type)
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 sample_object_pose_table(obj, stable_fs, bmin, bmax):
"""Samples a transform of the object so that it lies on in the given
bounding box bmin,bmax.
Args:
obj (RigidObjectModel)
stable_fs (list of lists): giving the stable faces of the object,
as in MP2.
bmin,bmax (3-vectors): the bounding box of the area in which the
objects should lie.
"""
table_height = bmin[2] + 0.001
face = random.choice(stable_fs)
normal = np.cross(face[1] - face[0], face[2] - face[0])
normal = normal / np.linalg.norm(normal)
centroid = np.sum(face, axis=0) / len(face)
x = random.uniform(bmin[0], bmax[0])
y = random.uniform(bmin[1], bmax[1])
z = table_height + np.dot(centroid, normal)
Rnormal = so3.canonical((-normal).tolist())
Rx = so3.rotation((1, 0, 0), random.uniform(0, math.pi * 2))
Rxz = so3.rotation((0, 1, 0), -math.pi * 0.5)
R = so3.mul(Rxz, so3.mul(Rx, so3.inv(Rnormal)))
#R*com + t = [x,y,_]
t = vectorops.sub([x, y, z], so3.apply(R, obj.getMass().getCom()))
t[2] = z
obj.setTransform(R, t)
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 output_and_advance(self, **inputs):
try:
q = inputs['q']
dq = inputs['dq']
u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt'])
except KeyError:
print "Warning, cannot debug motion model, dq or dqcmd not in input"
return None
if self.dqpredlast != None:
if self.activeDofs != None:
dq = dq[:]
for i in [
i for i in range(len(q)) if i not in self.activeDofs
]:
dq[i] = self.dqpredlast[i]
#compare motion model to dq
print "Motion model error:", np.linalg.norm(self.dqpredlast -
np.array(dq))
(v, i) = max(
zip(
np.abs(self.dqpredlast - np.array(dq)).tolist(),
range(len(dq))))
print " Max error:", v, "at", i,
if self.robot != None: print self.robot.link(i).getName()
else: print
print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[
i], "Actual:", dq[i]
print " pred:", self.Alast[i, i], "*u +", self.blast[i]
def grasp_from_contacts(contact1,contact2):
"""Helper: if you have two contacts, this returns an AntipodalGrasp"""
d = vectorops.unit(vectorops.sub(contact2.x,contact1.x))
grasp = AntipodalGrasp(vectorops.interpolate(contact1.x,contact2.x,0.5),d)
grasp.finger_width = vectorops.distance(contact1.x,contact2.x)
grasp.contact1 = contact1
grasp.contact2 = contact2
return grasp
def simulateForceSpring(self, kP=10.0):
if not self.forceApplicationMode: return
self.sim.updateWorld()
body = self.sim.body(self.forceObject)
T = self.forceObject.getTransform()
wp = se3.apply(T, self.forceLocalPoint)
f = vectorops.mul(vectorops.sub(self.forceAnchorPoint, wp), kP)
body.applyForceAtLocalPoint(f, self.forceLocalPoint)
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 taskDifference(self, a, b):
if self.taskType == 'po':
return se3.error(a, b)
elif self.taskType == 'position':
return vectorops.sub(a, b)
elif self.taskType == 'orientation':
return so3.error(a, b)
else:
raise ValueError("Invalid taskType " + self.taskType)
def make_object_arrangement(world,
container,
objects,
container_wall_thickness=0.01,
max_iterations=100,
remove_failures=False):
"""For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations
and z orientations so that they are initially collision free and on the bottom of the container.
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.
max_iterations (int, optional): the maximum number of iterations used
for sampling object initial poses
remove_failures (bool): if True, then instead of returning None on
failure, the objects that fail placement are removed from the world.
Returns:
(WorldModel): if successful, the positions of objects in world are
modified and world is returned. On failure, None is returned.
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))
collision_margin = 0.0025
for object in objects:
#make sure the bottom of the object touches the bottom of the container
obb = _get_bound(object)
zmin = obb[0][2]
R, t = object.getTransform()
t[2] = container_inner_bb[2] - zmin + collision_margin
object.setTransform(R, t)
failures = xy_jiggle(world, rigid_objects, [container],
container_inner_bb[0], container_inner_bb[1],
max_iterations)
if len(failures) > 0:
if remove_failures:
removeIDs = [objects[i].index for i in failures]
for i in sorted(removeIDs)[::-1]:
world.remove(world.rigidObject(i))
else:
return None
return world
def taskDifference(self,a,b):
if self.taskType == 'po':
return se3.error(a,b)
elif self.taskType == 'position':
return vectorops.sub(a,b)
elif self.taskType == 'orientation':
return so3.error(a,b)
else:
raise ValueError("Invalid taskType "+self.taskType)
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 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 value(self, x, y, z):
result = np.zeros(6)
if len(y) == 0:
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
n = len(y)
m = len(z) // n
assert m == 7
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
f_normal = z[m * (i + 1) - 1]
f_friction = z[m * i:m * (i + 1) - 1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
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)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Normal_friction)
result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Cross_Normal_friction)
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
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 error_analysis(self):
"""
:return: None
"""
Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])
Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])
trans_error_list = []
trans_error_mat = []
rot_error_list = []
rot_error_mat = []
redraw_list = []
for i in range(len(self.Tlink_set)):
Toct_needle_est = se3.mul(
se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle)
trans_error = vectorops.sub(
se3.inv(self.trans_list[i])[1], Toct_needle_est[1])
rot_error = so3.error(
se3.inv(self.trans_list[i])[0], Toct_needle_est[0])
trans_error_list.append(vectorops.normSquared(trans_error))
trans_error_mat.append(np.absolute(trans_error))
rot_error_list.append(vectorops.normSquared(rot_error))
rot_error_mat.append(np.absolute(rot_error))
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[0])
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[1])
redraw_list.append(
create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0]))
draw_geometries(redraw_list)
trans_error_list = np.array(trans_error_list)
trans_error_mat = np.array(trans_error_mat)
rot_error_list = np.array(rot_error_list)
rot_error_mat = np.array(rot_error_mat)
rmse_trans = np.sqrt(np.mean(trans_error_list))
rmse_rot = np.sqrt(np.mean(rot_error_list))
print("The squared translation error list is:\n ",
np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans)
print("The mean error in XYZ directions is: ",
np.mean(trans_error_mat, axis=0))
print("The squared rotation error list is:\n ",
np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot)
print("The mean error in three rotation vectors is: ",
np.mean(rot_error_mat, axis=0))
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list),
# np.sqrt(rot_error_list)]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0),
# np.mean(rot_error_mat, axis=0)]), allow_pickle=True)
trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world):
global lower_corner,upper_corner
resolution = (15,15,15)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
for i in range(resolution[0]):
for j in range(resolution[1]):
for k in range(resolution[2]):
orig_config = robot.getConfig()
point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner)
world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world)
local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local)
point_goal = ik.objective(end_effector, local=point_local, world=point)
if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10):
reachable[i,j,k] = 1.0
robot.setConfig(orig_config)
return reachable
def mahalanobis_distance2(u,v,A=None):
if A is None:
return vectorops.distanceSquared(u,v)
else:
z = np.array(vectorops.sub(u,v))
return np.dot(z.T,np.dot(A,z))
def taskDifference(self,a,b): """Default: assumes a Cartesian space""" return vectorops.sub(a,b)
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 make_object_arrangement(world,container,objects,container_wall_thickness=0.01,max_iterations=100,remove_failures=False):
"""For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations
and z orientations so that they are initially collision free and on the bottom of the container.
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.
max_iterations (int, optional): the maximum number of iterations used
for sampling object initial poses
remove_failures (bool): if True, then instead of returning None on
failure, the objects that fail placement are removed from the world.
Returns:
(WorldModel): if successful, the positions of objects in world are
modified and world is returned. On failure, None is returned.
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))
collision_margin = 0.0025
for object in objects:
#make sure the bottom of the object touches the bottom of the container
obb = _get_bound(object)
zmin = obb[0][2]
R,t = object.getTransform()
t[2] = container_inner_bb[2] - zmin + collision_margin
object.setTransform(R,t)
failures = xy_jiggle(world,rigid_objects,[container],container_inner_bb[0],container_inner_bb[1],max_iterations)
if len(failures) > 0:
if remove_failures:
removeIDs = [objects[i].index for i in failures]
for i in sorted(removeIDs)[::-1]:
world.remove(world.rigidObject(i))
else:
return None
return world
