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 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 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 transform_point_cloud(pc,
T,
point_channels=[0, 1, 2],
normal_channels=[6, 7, 8]):
"""Given a point cloud `pc` and a transform T, apply the transform
to the point cloud (in place).
Args:
pc (np.ndarray): an N x M numpy array, with N points and M
channels.
T (klampt se3 element): a Klamp't se3 element representing
the transform to apply.
point_channels (list of 3 ints): The channel indices (columns)
in pc corresponding to the point data.
normal_channels (list of 3 ints): The channels indices(columns)
in pc corresponding to the normal data. If this is None
or an index is >= M, just ignore.
"""
N, M = pc.shape
assert len(point_channels) == 3
for i in point_channels:
assert i < M, "Invalid point_channel"
for i in range(N):
point_data = pc[i, :]
point = point_data[point_channels]
point_data[point_channels] = se3.apply(T, point)
if normal_channels is not None and normal_channels[0] < M:
for i in normal_channels:
assert i < M, "Invalid normal_channel"
for i in range(N):
point_data = pc[i, :]
point = point_data[normal_channels]
point_data[normal_channels] = so3.apply(T[0], point)
def display(self):
T1 = self.taskGen.getDesiredCartesianPose('left',0)
T2 = self.taskGen.getDesiredCartesianPose('right',1)
baseTransform = self.taskGen.world.robot(0).link(2).getTransform()
glEnable(GL_LIGHTING)
if T1 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T1),0.2,0.03,fancy=True,lighting=True)
if T2 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T2),0.2,0.03,fancy=True,lighting=True)
dstate = self.taskGen.serviceThread.hapticupdater.deviceState
if T1 is not None and dstate[0] != None:
R = dstate[0].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T1[1]))
self.draw_wand(T)
if T2 is not None and dstate[1] != None:
R = dstate[1].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T2[1]))
self.draw_wand(T)
if debugHapticTransform:
for deviceState in dstate:
#debugging
#this is the mapping from the handle to the user frame
Traw = (so3.from_moment(deviceState.rotationMoment),deviceState.position)
Tuser = se3.mul(se3.inv(handleToUser),Traw)
T = se3.mul(userToWorld,se3.mul(Tuser,worldToUser))
T = (T[0],so3.apply([0,-1,0, 0,0,1, -1,0,0],T[1]))
T = deviceToViewTransform(Traw[0],Traw[1])
gldraw.xform_widget(se3.mul(se3translation([-1,0,0]),Traw),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),Tuser),0.5,0.05,lighting=True,fancy=True)
#gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),se3.mul(Traw,worldToHandle)),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(T,0.5,0.05,lighting=True,fancy=True)
break
def axis_rotation_magnitude(R, a):
"""Given a rotation matrix R and a unit axis a, determines how far R rotates
about a. Specifically, if R = from_axis_angle((a,v)) this should return v (modulo pi).
"""
cterm = so3.trace(R) - vectorops.dot(a, so3.apply(R, a))
mR = so3.matrix(R)
sterm = -(a[0] * (mR[1][2] - mR[2][1]) + a[1] *
(mR[2][0] - mR[0][2]) + a[2] * (mR[0][1] - mR[1][0]))
return math.atan2(sterm, cterm)
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 getSpheres(self):
for i in range(len(self.obsList)):
obsDim = self.obsDimList[i]
pos = self.obsList[i]
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
obsSphere = so3.apply(
rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2])
obsSphere = np.array(obsSphere)
obsSphere = obsSphere + [pos[0], pos[1], pos[2]]
self.spherePosList.append(obsSphere)
return self.spherePosList
def checkCollision(self, q):
collisionFlag = False
# Hardcoded robot dimensions
robDim = [0.05, 0.05, 0.05]
robSphereRad = self.getRadius(robDim)
rotMat = mathUtils.euler_zyx_mat([q[3], q[4], q[5]])
# position is half of the robots dimension - Center of the approximated robot when placed at [0,0,0]
robSphere = so3.apply(rotMat,
[robDim[0] / 2, robDim[1] / 2, robDim[2] / 2])
robSphere = np.array(robSphere)
robSphere = robSphere + [q[0], q[1], q[2]]
if len(self.spherePosList) == len(self.obsList):
for i in range(len(self.spherePosList)):
obsDim = self.obsDimList[i]
obsSphereRad = self.getRadius(obsDim)
obsSphere = self.spherePosList[i]
dist = np.sqrt((obsSphere[0] - robSphere[0])**2 +
(obsSphere[1] - robSphere[1])**2 +
(obsSphere[2] - robSphere[2])**2)
if (dist <= robSphereRad + obsSphereRad + self.epsilon):
collisionFlag = True
break
return collisionFlag
for i in range(len(self.obsList)):
obsDim = self.obsDimList[i]
obsSphereRad = self.getRadius(obsDim)
pos = self.obsList[i]
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
obsSphere = so3.apply(
rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2])
obsSphere = np.array(obsSphere)
obsSphere = obsSphere + [pos[0], pos[1], pos[2]]
dist = np.sqrt((obsSphere[0] - robSphere[0])**2 +
(obsSphere[1] - robSphere[1])**2 +
(obsSphere[2] - robSphere[2])**2)
if (dist <= robSphereRad + obsSphereRad):
collisionFlag = True
break
return collisionFlag
def so3_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = so3_staggered_grid(N)
else:
G = so3_grid(N)
vispt = [1, 0, 0]
for n in G.nodes():
R = G.node[n]['params']
trans = so3.apply(R, vispt)
#trans = so3.axis_angle(R)[0]
#trans = vectorops.unit(so3.quaternion(R)[:3])
vis.add(str(n), [R, trans])
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
Ri = G.node[i]['params']
Rj = G.node[j]['params']
tmax = 9
times = range(tmax + 1)
milestones = []
for t in times:
u = float(t) / tmax
trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
milestones.append(trans)
vis.add(
str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
vis.hideLabel(str(i) + '-' + str(j))
dist = so3.distance(Ri, Rj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def euler_zyx_mat(theta):
"""For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that
A*dtheta is the angular velocities when dtheta is the rate of change of the
euler angles"""
eu = [0, 0, 1]
ev = [0, 1, 0]
ew = [1, 0, 0]
Ru = so3.rotation([0, 0, 1], theta[0])
Rv = so3.rotation([0, 1, 0], theta[1])
col1 = eu
col2 = so3.apply(Ru, ev)
col3 = so3.apply(Ru, so3.apply(Rv, ew))
#col1 = [0,0,1]
#col2 = [c0 -s0 0] [0] = [-s0]
# [s0 c0 0]*[1] [c0 ]
# [0 0 1] [0] [0 ]
#col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0]
# [0 1 0 ]*[0] [0 ] [c1s0]
# [-s1 0 c1] [0] [-s1] [-s1 ]
#A = [ 0 -s0 c1c0]
# [ 0 c0 c1s0]
# [ 1 0 -s1 ]
#return [col1[2], col2[2], col3[2], col1[1], col2[1], col3[1], col1[0], col2[0], col3[0]]
phi = theta[0]
tht = theta[1]
psi = theta[2]
cphi = math.cos(phi)
sphi = math.sin(phi)
ctht = math.cos(tht)
stht = math.sin(tht)
cpsi = math.cos(psi)
spsi = math.sin(psi)
rotMat = [
cphi * ctht, sphi * cpsi + cphi * stht * spsi,
sphi * spsi - cphi * stht * cpsi, -sphi * ctht,
cphi * cpsi - sphi * stht * spsi, cphi * spsi + sphi * stht * cpsi,
stht, -spsi * ctht, cpsi * ctht
]
return rotMat
def euler_zyx_moments(theta):
"""For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that
A*dtheta is the angular velocities when dtheta is the rate of change of the
euler angles"""
eu = [0, 0, 1]
ev = [0, 1, 0]
ew = [1, 0, 0]
Ru = so3.rotation([0, 0, 1], theta[0])
Rv = so3.rotation([0, 1, 0], theta[1])
col1 = eu
col2 = so3.apply(Ru, ev)
col3 = so3.apply(Ru, so3.apply(Rv, ew))
#col1 = [0,0,1]
#col2 = [c0 -s0 0] [0] = [-s0]
# [s0 c0 0]*[1] [c0 ]
# [0 0 1] [0] [0 ]
#col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0]
# [0 1 0 ]*[0] [0 ] [c1s0]
# [-s1 0 c1] [0] [-s1] [-s1 ]
#A = [ 0 -s0 c1c0]
# [ 0 c0 c1s0]
# [ 1 0 -s1 ]
return zip(col1, col2, col3)
def solve_placement(self):
"""Implemented for you: come up with a collision-free target placement"""
obmin,obmax = self.objbb
ozmin = obmin[2]
min_dims = min(obmax[0]-obmin[0],obmax[1]-obmin[1])
center = [0.5*(obmax[0]+obmin[0]),0.5*(obmax[1]-obmin[1])]
xmin,ymin,zmin = self.goal_bounds[0]
xmax,ymax,zmax = self.goal_bounds[1]
center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)]
Tobj_feasible = []
for iters in range(20):
crand = [random.uniform(b[0],b[1]) for b in center_sampling_range]
Robj = so3.rotation((0,0,1),random.uniform(0,math.pi*2))
tobj = vectorops.add(so3.apply(Robj,[-center[0],-center[1],0]),[crand[0],crand[1],zmin-ozmin+0.002])
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
bmin,bmax = self.object.geometry().getBBTight()
if bmin[0] < xmin:
tobj[0] += xmin-bmin[0]
if bmax[0] > xmax:
tobj[0] -= bmin[0]-xmax
if bmin[1] < ymin:
tobj[1] += ymin-bmin[1]
if bmax[1] > ymax:
tobj[1] -= bmin[1]-ymax
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
continue
for i in range(self.world.numRigidObjects()):
if i == self.object.index: continue
if self.object.geometry().collides(self.world.rigidObject(i).geometry()):
#raise it up a bit
bmin,bmax = self.world.rigidObject(i).geometry().getBB()
tobj[2] = bmax[2]-ozmin+0.002
self.object.setTransform(Robj,tobj)
Tobj_feasible.append((Robj,tobj))
print("Found",len(Tobj_feasible),"valid object placements")
return Tobj_feasible
def retract(robot,gripper,amount,local=True):
"""Retracts the robot's gripper by a vector `amount`.
if local=True, amount is given in local coordinates. Otherwise, its given in
world coordinates.
"""
if not isinstance(gripper,(int,str)):
gripper = gripper.base_link
link = robot.link(gripper)
Tcur = link.getTransform()
if local:
amount = so3.apply(Tcur[0],amount)
obj = ik.objective(link,R=Tcur[0],t=vectorops.add(Tcur[1],amount))
res = ik.solve(obj)
if not res:
return None
return robot.getConfig()
def next(self):
"""Returns the next Grasp from the database."""
if self.options is None:
return None
if self.index >= len(self.options):
self.options = None
return None
c, n, score = self.options(self.index)
self.index += 1
cworld = se3.apply(self.pc_xform, c)
nworld = so3.apply(self.pc_xform[0], n)
objective = IKObjective()
objective.setLinks(self.gripper.link)
objective.setFixedPoint(self.gripper.center, cworld)
objective.setAxialRotConstraint(self.gripper.primary_axis,
vectorops.mul(nworld, -1))
return Grasp(objective, score=score)
def plan_pick_one(world,robot,object,gripper,grasp, robot0=True):
"""
Plans a picking motion for a given object and a specified grasp.
Arguments:
world (WorldModel): the world, containing robot, object, and other items that
will need to be avoided.
robot (RobotModel): the robot in its current configuration
object (RigidObjectModel): the object to pick.
gripper (GripperInfo): the gripper.
grasp (Grasp): the desired grasp. See common/grasp.py for more information.
Returns:
None or (transit,approach,lift): giving the components of the pick motion.
Each element is a RobotTrajectory. (Note: to convert a list of milestones
to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones)
Tip:
vis.debug(q,world=world) will show a configuration.
"""
qstart = robot.getConfig()
qstartopen = gripper.set_finger_config(qstart, gripper.partway_open_config(1)) # open the fingers of the start to match qpregrasp
robot.setConfig(qstartopen)
grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module
def feasible():
return is_collision_free_grasp(world, robot, object)
# return True
solved = ik.solve_global(objectives=grasp.ik_constraint, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7],feasibilityCheck=feasible)
print('ik status:', solved)
if not solved: return None
qpregrasp = robot.getConfig()
robot.setConfig(qpregrasp)
if not feasible(): return None
qtransit = retract(robot=robot, gripper=gripper, amount=list(-0.1*np.array(gripper.primary_axis)), local=True)
# rotate the gripper along its primary axis to make its secondary axis perpendicular to the object
# so that it can grasp the object easily
secondary_axis_gripper = gripper.secondary_axis
if not isinstance(gripper,(int,str)):
gripper1 = gripper.base_link
else:
gripper1 = gripper
link = robot.link(gripper1)
R_gripper_w, _ = link.getTransform()
secondary_axis_world = so3.apply(R_gripper_w, secondary_axis_gripper)
secondary_axis_world_2d = np.array(secondary_axis_world)[:-1]
angle = np.arccos(np.dot(secondary_axis_world_2d, [0, 1]))
q_rotate = copy.deepcopy(qtransit)
q_rotate[7] = clip_angle(q_rotate[7] - angle)
qpregrasp[7] = clip_angle(qpregrasp[7] - angle)
if qtransit is None: return None
print(qtransit)
robot.setConfig(qtransit)
if not feasible(): return None
robot.setConfig(qpregrasp)
qopen = gripper.set_finger_config(qpregrasp, gripper.partway_open_config(1)) # open the fingers further
robot.setConfig(qopen)
if not feasible(): return None
if robot0:
close_amount = 0.1
else:
close_amount = 0.8
qgrasp = gripper.set_finger_config(qopen, gripper.partway_open_config(close_amount)) # close the gripper
robot.setConfig(qgrasp)
qlift = retract(robot=robot, gripper=gripper, amount=[0, 0, 0.1], local=False)
robot.setConfig(qlift)
if not feasible(): return None
robot.setConfig(qstart)
transit = feasible_plan(world, robot, object, qtransit) # decide whether to use feasible_plan or optimizing_plan
if not transit:
return None
return RobotTrajectory(robot,milestones=[qstart]+transit),\
RobotTrajectory(robot,milestones=[qtransit, q_rotate, qpregrasp, qopen, qgrasp]),\
RobotTrajectory(robot,milestones=[qgrasp,qlift])
def integrate_velocities(controller, sim, dt):
"""The make() function returns a 1-argument function that takes a SimRobotController and performs whatever
processing is needed when it is invoked."""
global start_time
global now_dur
global syn_curr
global syn_next
global entered_once
global got_syn
if not entered_once:
syn_vec = controller.getCommandedConfig()
if not syn_vec:
got_syn = False
entered_once = False
else:
got_syn = True
entered_once = True
syn_curr = syn_vec[34]
start_time = sim.getTime()
else:
syn_curr = syn_next
now_dur = sim.getTime() - start_time
# print 'The current simulation duration is', now_dur
rob = sim.controller(0).model()
palm_curr = mv_el.get_moving_base_xform(rob)
R = palm_curr[0]
t = palm_curr[1]
if DEBUG:
print 'The current palm rotation is ', R
print 'The current palm translation is ', t
print 'The simulation dt is ', dt
# Converting to rpy
euler = so3.rpy(R)
# Checking if list empty and returning false
if not got_syn:
return False, None, None
if DEBUG:
print 'The integration time is ', dt
print 'The adaptive velocity of hand is ', global_vars.hand_command
print 'The adaptive twist of palm is \n', global_vars.arm_command
# print 'The commanded position of the hand encoder (in memory) is ', syn_curr
# print 'The present position of the hand encoder (sensed) is ', controller.getCommandedConfig()[34]
v = rob.getVelocity()
print 'The actual velocity of the robot is ', v
# print 'The present pose of the palm is \n', palm_curr
# print 'The present position of the palm is ', t, 'and its orientation is', euler
# Getting linear and angular velocities
lin_vel_vec = global_vars.arm_command.linear
ang_vel_vec = global_vars.arm_command.angular
lin_vel = [lin_vel_vec.x, lin_vel_vec.y, lin_vel_vec.z]
ang_vel = [ang_vel_vec.x, ang_vel_vec.y, ang_vel_vec.z]
ang_vel_loc = so3.apply(so3.inv(R),
ang_vel) # rotating ang vel to local frame
# Transform ang vel into rpy vel
euler_vel = transform_ang_vel(euler, ang_vel_loc)
palm_vel = []
palm_vel.extend(lin_vel)
palm_vel.extend([euler_vel[0], euler_vel[1], euler_vel[2]]) # appending
syn_vel = global_vars.hand_command
# Integrating
syn_next = syn_curr + syn_vel * int_const_syn * dt
t_next = vectorops.madd(t, lin_vel, int_const_t * dt)
euler_next = vectorops.madd(euler, euler_vel, int_const_eul * dt)
# Saturating synergy
if syn_next >= 1.0:
syn_next = 1.0
# Convert back for send xform
palm_R_next = so3.from_rpy(euler_next)
palm_t_next = t_next
palm_next = (palm_R_next, palm_t_next)
if DEBUG or True:
print 'euler is ', euler, ' and is of type ', type(euler)
print 'euler_vel is ', euler_vel, ' and is of type ', type(euler_vel)
print 'euler_next is ', euler_next, ' and is of type ', type(
euler_next)
#
# print 't is ', t, ' and is of type ', type(t)
# print 't_next is ', t_next, ' and is of type ', type(t_next)
#
# print 'R is ', R, ' and is of type ', type(R)
# print 'palm_R_next is ', palm_R_next, ' and is of type ', type(palm_R_next)
#
# print 'palm_curr is ', palm_curr, ' and is of type ', type(palm_curr)
# print 'palm_next is ', palm_next, ' and is of type ', type(palm_next)
return True, syn_next, palm_next, syn_vel, palm_vel
def stable_faces(obj, com=None, stability_tol=0.0, merge_tol=0.0):
"""
Returns a list of support polygons on the object that are
likely to be stable on a planar surface.
Args:
obj (RigidObjectModel or Geometry3D): the object.
com (3-list, optional): sets the local center of mass. If
not given, the default RigidObjectModel's COM will be used,
or (0,0,0) will be used for a Geometry3D.
stability_tol (float,optional): if > 0, then only faces that
are stable with all perturbed "up" directions (dx,dy,1) with
||(dx,dy)||<= normal_tol will be outputted (robust stability).
If < 0, then all faces that are stable from some "up" direction
(dx,dy,1) with ||(dx,dy)||<= |normal_tol| will be outputted
(non-robust stability)
merge_tol (float, optional): if > 0, then adjacent faces with
normals whose angles are within this tolerance (in rads) will
be merged together.
Returns:
list of list of 3-vectors: The set of all polygons that could
form stable sides. Each polygon is convex and listed in
counterclockwise order (i.e., the outward normal can be obtained
via:
(poly[2]-poly[0]) x (poly[1]-poly[0])
"""
if isinstance(obj, RigidObjectModel):
geom = obj.geometry()
if com is None:
com = obj.getMass().getCom()
else:
geom = obj
if com is None:
com = (0, 0, 0)
assert len(com) == 3, "Need to provide a 3D COM"
ch_trimesh = geom.convert('ConvexHull').convert('TriangleMesh')
xform, (verts, tris) = numpy_convert.to_numpy(ch_trimesh)
trinormals = get_triangle_normals(verts, tris)
edges = dict()
tri_neighbors = np.full(tris.shape, -1, dtype=np.int32)
for ti, tri in enumerate(tris):
for ei, e in enumerate([(tri[0], tri[1]), (tri[1], tri[2]),
(tri[2], tri[0])]):
if (e[1], e[0]) in edges:
tn, tne = edges[(e[1], e[0])]
if tri_neighbors[tn][tne] >= 0:
print("Warning, triangle", ti,
"neighbors two triangles on edge", tne, "?")
tri_neighbors[ti][ei] = tn
tri_neighbors[tn][tne] = ti
else:
edges[e] = ti, ei
num_empty_edges = 0
for ti, tri in enumerate(tris):
for e in range(3):
if tri_neighbors[tn][e] < 0:
num_empty_edges += 1
if num_empty_edges > 0:
print("Info: boundary of mesh has", num_empty_edges, "edges")
visited = [False] * len(tris)
cohesive_faces = dict()
for ti, tri in enumerate(tris):
if visited[ti]:
continue
face = [ti]
visited[ti] = True
myvisit = set()
myvisit.add(ti)
q = deque()
q.append(ti)
while q:
tvisit = q.popleft()
for tn in tri_neighbors[tvisit]:
if tn >= 0 and tn not in myvisit:
if math.acos(trinormals[ti].dot(
trinormals[tn])) <= merge_tol:
face.append(tn)
myvisit.add(tn)
q.append(tn)
for t in myvisit:
visited[t] = True
cohesive_faces[ti] = face
output = []
for t, face in cohesive_faces.items():
n = trinormals[t]
R = so3.canonical(n)
if len(face) > 1:
#project face onto the canonical basis
faceverts = set()
for t in face:
faceverts.add(tris[t][0])
faceverts.add(tris[t][1])
faceverts.add(tris[t][2])
faceverts = list(faceverts)
xypts = [so3.apply(so3.inv(R), verts[v])[1:3] for v in faceverts]
try:
ch = ConvexHull(xypts)
face = [faceverts[v] for v in ch.vertices]
except Exception:
print("Error computing convex hull of", xypts)
print("Vertex indices", faceverts)
print("Vertices", [verts[v] for v in faceverts])
else:
face = tris[face[0]]
comproj = np.array(so3.apply(so3.inv(R), com)[1:3])
stable = True
for vi in range(len(face)):
vn = (vi + 1) % len(face)
a, b = face[vi], face[vn]
pa = np.array(so3.apply(so3.inv(R), verts[a])[1:3])
pb = np.array(so3.apply(so3.inv(R), verts[b])[1:3])
#check distance from com
elen = np.linalg.norm(pb - pa)
if elen == 0:
continue
sign = np.cross(pb - pa, comproj - pa) / elen
if sign < stability_tol:
stable = False
break
if stable:
output.append([verts[i] for i in face])
return output
sim = Simulator(w)
robot = w.robot(0)
cam = sim.controller(0).sensor("rgbd_camera")
link = robot.link(robot.numLinks() - 1)
amplitudes = [random.uniform(0, 2) for i in range(robot.numLinks())]
phases = [random.uniform(0, math.pi * 2) for i in range(robot.numLinks())]
periods = [random.uniform(0.5, 2) for i in range(robot.numLinks())]
qmin, qmax = robot.getJointLimits()
vis.add("world", w)
vis.add("cam", cam)
vp = vis.getViewport()
vp.camera.rot[1] -= 0.5
vis.setViewport(vp)
default = vis.getViewport().getTransform()
print('x:', so3.apply(default[0], [1, 0, 0]))
print('y:', so3.apply(default[0], [0, 1, 0]))
print('z:', so3.apply(default[0], [0, 0, 1]))
print('offset:', default[1])
circle_points = []
npts = 50
times = []
for i in range(npts + 1):
angle = math.radians(360 * i / npts)
circle_points.append(
se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default))
times.append(i * 20 / npts)
circle_traj = SE3Trajectory(times, circle_points)
circle_traj.milestones[-1] = circle_traj.milestones[0]
circle_smooth_traj = SE3HermiteTrajectory()
circle_smooth_traj.makeSpline(circle_traj, loop=True)
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 viewToDeviceTransform(R, t):
Ruser = so3.mul(worldToUser[0], so3.mul(R, userToWorld[0]))
R = so3.mul(handleToUser[0], Ruser)
return (R, so3.apply(so3.inv([0, 1, 0, 0, 0, 1, 1, 0, 0]), t))
def deviceToViewTransform(R, t):
Ruser = so3.mul(so3.inv(handleToUser[0]), R)
R = so3.mul(userToWorld[0], so3.mul(Ruser, worldToUser[0]))
return (R, so3.apply([0, 1, 0, 0, 0, 1, 1, 0, 0], t))
def drive(self, qcur, angVel, vel, dt):
"""Drives the robot by an incremental time step to reach the desired
Cartesian (angular/linear) velocities of the links previously specified
in start().
Args:
qcur (list of floats): The robot's current configuration.
angVel (3-vector or list of 3-vectors): the angular velocity of
each driven link. Set angVel to None to turn off orientation
control of every constraint. angVel[i] to None to turn off
orientation control of constraint i.
vel (3-vector or list of 3-vectors): the linear velocity of each
driven link. Set vel to None to turn off position control of
every constraint. Set vel[i] to None to turn off position
control of constraint i.
dt (float): the time step.
Returns:
tuple: A pair ``(progress,qnext)``. ``progress`` gives a value
in the range [0,1] indicating indicating how far along the nominal
drive amount the solver was able to achieve. If the result is < 0,
this indicates that the solver failed to make further progress.
``qnext`` is the resulting configuration that should be sent to the
robot's controller.
For longer moves, you should pass qnext back to this function as qcur.
"""
if angVel is None:
#turn off orientation control
if vel is None:
#nothing specified
return (1.0, qcur)
angVel = [None] * len(self.links)
else:
assert len(angVel) > 0
if not hasattr(angVel[0], '__iter__'):
angVel = [angVel]
if vel is None:
#turn off position control
vel = [None] * len(self.links)
else:
assert len(vel) > 0
if not hasattr(vel[0], '__iter__'):
vel = [vel]
if len(qcur) != self.robot.numLinks():
raise ValueError(
"Invalid size of current configuration, %d != %d" %
(len(qcur), self.robot.numLinks()))
if len(angVel) != len(self.links):
raise ValueError(
"Invalid # of angular velocities specified, %d != %d" %
(len(angVel), len(self.links)))
if len(vel) != len(self.links):
raise ValueError(
"Invalid # of translational velocities specified, %d != %d" %
(len(vel), len(self.links)))
anyNonzero = False
zeroVec = [0, 0, 0]
for (w, v) in zip(angVel, vel):
if w is not None and list(w) != zeroVec:
anyNonzero = True
break
if v is not None and list(v) != zeroVec:
anyNonzero = True
break
if not anyNonzero:
return (1.0, qcur)
qout = [v for v in qcur]
#update drive transforms
self.robot.setConfig(qcur)
robotLinks = [self.robot.link(l) for l in self.links]
#limit the joint movement by joint limits and velocity
tempqmin, tempqmax = self.robot.getJointLimits()
if self.qmin is not None:
tempqmin = self.qmin
if self.qmax is not None:
tempqmax = self.qmax
vmax = self.robot.getVelocityLimits()
vmin = vectorops.mul(vmax, -1)
if self.vmin is not None:
vmin = self.vmin
if self.vmax is not None:
vmax = self.vmax
for i in range(self.robot.numLinks()):
tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i])
tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i])
#Setup the IK solver parameters
self.solver.setJointLimits(tempqmin, tempqmax)
tempGoals = [ik.IKObjective(g) for g in self.ikGoals]
for i in range(len(self.links)):
if math.isfinite(self.positionTolerance) and math.isfinite(
self.rotationTolerance):
tempGoals[i].rotationScale = self.positionTolerance / (
self.positionTolerance + self.rotationTolerance)
tempGoals[i].positionScale = self.rotationTolerance / (
self.positionTolerance + self.rotationTolerance)
elif not math.isfinite(
self.positionTolerance) and not math.isfinite(
self.rotationTolerance):
pass
else:
tempGoals[i].rotationScale = min(
self.positionTolerance,
self.rotationTolerance) / self.rotationTolerance
tempGoals[i].positionScale = min(
self.positionTolerance,
self.rotationTolerance) / self.positionTolerance
tolerance = min(
1e-6,
min(self.positionTolerance, self.rotationTolerance) /
(math.sqrt(3.0) * len(self.links)))
self.solver.setTolerance(tolerance)
#want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t)
#where TPath_i(t) = Advance(Tdrive_i,Screw_i*t)
if self.driveSpeedAdjustment == 0:
self.driveSpeedAdjustment = 0.1
anyFailures = False
while self.driveSpeedAdjustment > 0:
#advance the desired cartesian goals
#set up IK parameters: active dofs, IKGoal
amount = dt * self.driveSpeedAdjustment
desiredTransforms = [[None, None] for i in range(len(self.links))]
for i in range(len(self.links)):
if vel[i] is not None:
desiredTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], amount)
tempGoals[i].setFixedPosConstraint(
self.endEffectorOffsets[i], desiredTransforms[i][1])
else:
tempGoals[i].setFreePosition()
if angVel[i] is not None:
increment = so3.from_moment(
vectorops.mul(angVel[i], amount))
desiredTransforms[i][0] = so3.mul(
increment, self.driveTransforms[i][0])
tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0])
else:
tempGoals[i].setFreeRotConstraint()
self.solver.set(i, tempGoals[i])
err0 = self.solver.getResidual()
quality0 = vectorops.norm(err0)
res = self.solver.solve()
q = self.robot.getConfig()
activeDofs = self.solver.getActiveDofs()
for k in activeDofs:
if q[k] < tempqmin[k] or q[k] > tempqmax[k]:
#the IK solver normalizer doesn't care about absolute
#values for joints that wrap around 2pi
if tempqmin[k] <= q[k] + 2 * math.pi and q[
k] + 2 * math.pi <= tempqmax[k]:
q[k] += 2 * math.pi
elif tempqmin[k] <= q[k] - 2 * math.pi and q[
k] - 2 * math.pi <= tempqmax[k]:
q[k] -= 2 * math.pi
else:
print(
"CartesianDriveSolver: Warning, result from IK solve is out of bounds: index",
k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k])
q[k] = max(min(q[k], tempqmax[k]), tempqmin[k])
self.robot.setConfig(q)
#now evaluate quality of the solve
errAfter = self.solver.getResidual()
qualityAfter = vectorops.norm(errAfter)
if qualityAfter > quality0:
#print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter)
#print(" solver result was",res,"increment",amount)
res = False
elif qualityAfter < quality0 - 1e-8:
res = True
if res:
#success!
for k in activeDofs:
qout[k] = q[k]
assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k]
#now advance the driven transforms
#figure out how much to drive along screw
numerator = 0 # this will get sum of distance * screws
denominator = 0 # this will get sum of |screw|^2 for all screws
#result will be numerator / denominator
achievedTransforms = [
(l.getTransform()[0], l.getWorldPosition(ofs))
for l, ofs in zip(robotLinks, self.endEffectorOffsets)
]
#TODO: get transforms relative to baseLink
for i in range(len(self.links)):
if vel[i] is not None:
trel = vectorops.sub(achievedTransforms[i][1],
self.driveTransforms[i][1])
vellen = vectorops.norm(vel[i])
axis = vectorops.div(vel[i], max(vellen, 1e-8))
ut = vellen
tdistance = vectorops.dot(trel, axis)
tdistance = min(max(tdistance, 0.0), dt * vellen)
numerator += ut * tdistance
denominator += ut**2
if angVel[i] is not None:
Rrel = so3.mul(achievedTransforms[i][0],
so3.inv(self.driveTransforms[i][0]))
vellen = vectorops.norm(angVel[i])
angVelRel = so3.apply(
so3.inv(self.driveTransforms[i][0]), angVel[i])
rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8))
Rdistance = axis_rotation_magnitude(Rrel, rotaxis)
Rdistance = min(max(Rdistance, 0.0), dt * vellen)
uR = vellen
numerator += uR * Rdistance
denominator += uR**2
distance = numerator / max(denominator, 1e-8)
#computed error-minimizing distance along screw motion
for i in range(len(self.links)):
if vel[i] is not None:
self.driveTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], distance)
else:
self.driveTransforms[i][1] = achievedTransforms[i][1]
if angVel[i] is not None:
Rincrement = so3.from_moment(
vectorops.mul(angVel[i], distance))
self.driveTransforms[i][0] = normalize_rotation(
so3.mul(Rincrement, self.driveTransforms[i][0]))
else:
self.driveTransforms[i][0] = achievedTransforms[i][0]
if math.ceil(distance / dt * 10) < math.floor(
self.driveSpeedAdjustment * 10):
self.driveSpeedAdjustment -= 0.1
self.driveSpeedAdjustment = max(self.driveSpeedAdjustment,
0.0)
elif not anyFailures:
#increase drive velocity
if self.driveSpeedAdjustment < 1.0:
self.driveSpeedAdjustment += 0.1
self.driveSpeedAdjustment = min(
1.0, self.driveSpeedAdjustment)
return (distance / dt, qout)
else:
#IK failed: try again with a slower speed adjustment
anyFailures = True
self.driveSpeedAdjustment -= 0.1
if self.driveSpeedAdjustment <= 1e-8:
self.driveSpeedAdjustment = 0.0
break
#no progress
return (-1, qcur)
