def __init__(self, armName, rand=False, x=.03, y=.05, z=.004, testAngles=False, maxPoses=1000, speed=0.01):
self.ravenArm = RavenArm(armName, defaultPoseSpeed=speed)
self.arm = armName
if armName == 'R':
self.homePose = tfx.pose([-.12,-.02,-.135],tfx.tb_angles(-90,90,0))
else:
self.homePose = tfx.pose([-.03,-.02,-.135],tfx.tb_angles(-90,90,0))
self.random = rand
print self.homePose
self.grasp = 1.2
joints_dict = kinematics.invArmKin(self.arm, self.homePose, self.grasp)
print joints_dict
self.x_levels = 5
self.y_levels = 5
self.z_levels = 7
self.x = x
self.y = y
self.z = z
self.maxPoses = maxPoses
# generate all angle combinations in order to sample the rotations as well
self.angles = []
self.angles.append(tfx.tb_angles(-90,90,0))
if False:
self.angles.append(tfx.tb_angles(-90,80,0))
self.angles.append(tfx.tb_angles(-90,100,0))
self.angles.append(tfx.tb_angles(-80,90,0))
self.angles.append(tfx.tb_angles(-80,80,0))
self.angles.append(tfx.tb_angles(-80,100,0))
self.angles.append(tfx.tb_angles(-100,90,0))
self.angles.append(tfx.tb_angles(-100,80,0))
self.angles.append(tfx.tb_angles(-100,100,0))
self.grid = []
self.grid += [self.homePose + [float(i)*(x/self.x_levels),0,0] for i in xrange(self.x_levels)]
self.grid += [self.homePose + [x, float(i)*(-y/self.y_levels),0] for i in xrange(self.x_levels)]
self.grid += [self.homePose + [-float(i)*(x/self.x_levels),-y,0] for i in xrange(self.x_levels)]
self.grid += [self.homePose + [0, -float(i)*(-y/self.y_levels),0] for i in xrange(self.x_levels)]
"""
self.grid = []
self.grid += [tfx.pose([x/self.x_levels,0,0]) for _ in xrange(self.x_levels)]
self.grid += [tfx.pose([0,-y/self.y_levels,0]) for _ in xrange(self.y_levels)]
self.grid += [tfx.pose([-x/self.x_levels,0,0]) for _ in xrange(self.x_levels)]
self.grid += [tfx.pose([0,y/self.y_levels,0]) for _ in xrange(self.y_levels)]
"""
"""
def _getUnallocatedCenters(self, armName, centers, new=False, forHasFoam=False):
unallocCenters = []
numAlloc = collections.defaultdict(int)
for center in centers:
ok = True
print tfx.pose(center)
foam_ik_valid = kinematics.invArmKin(armName, tfx.pose(center,tfx.tb_angles(-90,90,0)), math.pi/4.0) is not None
lift_ik_valid = kinematics.invArmKin(armName, tfx.pose(center+[0,0,.06],tfx.tb_angles(-90,90,0)), math.pi/4.0) is not None
if not foam_ik_valid:
ok = False
self.event_pub.publish(String('Cannot allocate foam piece for arm {0} because IK invalid for pose {1}'.format(armName,center)))
if not lift_ik_valid:
ok = False
self.event_pub.publish(String('Cannot allocate foam piece for arm {0} because IK invalid for move vertical pose {1}'.format(armName,center+[0,0,.06])))
for allocArm, allocationCenter in self.allocations.iteritems():
if allocationCenter is not None and allocationCenter.distance(center) < self.allocationRadius:
print 'allocationCenter {0}'.format(allocationCenter)
numAlloc[allocArm] += 1
ok = ok and (allocArm == armName and new)
#if allocationCenter is not None and allocationCenter.distance(center) < self.allocationRadius and \
# not (not new and allocArm == armName):
# ok = False
if ok:
unallocCenters.append(center)
if forHasFoam:
if unallocCenters:
return True
numAlloc.pop(armName,None)
for n in numAlloc.values():
if n >= 1:
return True
return False
else:
return unallocCenters
def getJointsFromPose(self, armName, pose, grasp, quiet=False):
"""
Calls IK server and returns a dictionary of {jointType : jointPos}
jointType is from raven_2_msgs.msg.Constants
jointPos is position in radians
Needs to return finger1 and finger2
"""
pose = raven_util.convertToFrame(tfx.pose(pose), self.refFrame)
joints = kin.invArmKin(armName, pose, grasp)
if joints is None:
rospy.loginfo('IK failed!')
if quiet:
return None
else:
raise RuntimeError("IK failed!")
return joints
