def __init__(self, transform, joint_angles):
self.transform_ = transform
self.defaultAngles_ = [0.0, 0.0, 0.0]
if joint_angles is None: joint_angles = self.defaultAngles_
self.jointAngles_ = joint_angles
self.transform_ = b2Transform()
self.transform_.angle = 0.0
self.transform_.position = (0, 0)
self.w = 2.0
self.h = 7.5
w = self.w
h = self.h
# Link One
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
transform.position = (0, h/2.0)
self.L1_ = Polygon(vertices, transform)
# Link Two
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 7)
transform.position = (0, 3*h/2.0)
self.L2_ = Polygon(vertices, transform)
# Link 3
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 14)
transform.position = (0, 5*h/2.0)
self.L3_ = Polygon(vertices, transform)
# Link 4
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 21)
transform.position = (0, 7*h/2.0)
self.L4_ = Polygon(vertices, transform)
self.joint1_ = RobotRevoluteJoint((0, h), self.jointAngles_[0],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joint2_ = RobotRevoluteJoint((0, 2*h), self.jointAngles_[1],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joint3_ = RobotRevoluteJoint((0, 3*h), self.jointAngles_[2],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joints_ = [self.joint1_, self.joint2_, self.joint3_]
self.gripper_ = RobotGripper(self.transform_, h, w)
# filter the objects which have been generated grasps
file_list = []
for file in file_list_tmp:
grasp_file_name = os.path.join(file, "grasps.pickle")
if not os.path.exists(grasp_file_name):
file_list.append(file)
if len(file_list) == 0: # all objects have generated grasps
print("[INFO] All objects have been generated grasps!")
exit(0)
object_numbers = len(file_list)
print("[file_list]:", file_list, "obj_num:", object_numbers, "\n")
sample_config = YamlConfig("./config/sample_config.yaml")
gripper = RobotGripper.load("./config/gripper_params.yaml")
fc_list = [
3.0, 2.0, 1.7, 1.4, 1.3, 1.2, 1.1, 1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4,
0.3
] # 15
# | | |
print("[fc_list]", fc_list, "len:", len(fc_list))
# test a worker
# good_grasp = []
# worker(1, 1, good_grasp)
# exit()
start_time = time.time()
job_list = np.arange(object_numbers)
from foxarm.common.keys import *
from foxarm.common.sdf_file import SdfFile
from foxarm.common.obj_file import ObjFile
from foxarm.common.database import Hdf5Database
def prn_obj(obj):
print('\n'.join(['*** %s ***\n%s\n' % item for item in obj.__dict__.items()]))
CONFIG = YamlConfig(TEST_CONFIG_NAME)
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME, os.path.join(WORK_DIR, "foxarm/common"))
ags = AntipodalGraspSampler(gripper, CONFIG)
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
dataset = database.dataset(TEST_DS_NAME)
##### Generate Grasps #####
print('\n\n*****\tObject: %s\t*****' % dataset.object_keys[0])
unaligned_grasps = ags.generate_grasps(obj, target_num_grasps=100)
print('### Generated %d unaligned grasps! ###' % len(unaligned_grasps))
grasps = {}
stps = dataset.stable_poses(dataset.object_keys[0])
print('### Generated %d stable poses! ###' % len(stps))
for stp in stps:
class RobotArm:
def __init__(self, transform, joint_angles):
self.transform_ = transform
self.defaultAngles_ = [0.0, 0.0, 0.0]
if joint_angles is None: joint_angles = self.defaultAngles_
self.jointAngles_ = joint_angles
self.transform_ = b2Transform()
self.transform_.angle = 0.0
self.transform_.position = (0, 0)
self.w = 2.0
self.h = 7.5
w = self.w
h = self.h
# Link One
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
transform.position = (0, h/2.0)
self.L1_ = Polygon(vertices, transform)
# Link Two
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 7)
transform.position = (0, 3*h/2.0)
self.L2_ = Polygon(vertices, transform)
# Link 3
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 14)
transform.position = (0, 5*h/2.0)
self.L3_ = Polygon(vertices, transform)
# Link 4
vertices = vertices=[(0,0),(w,0),(w,h),(0,h)]
transform = b2Transform()
transform.angle = self.transform_.angle
# position = (0, 21)
transform.position = (0, 7*h/2.0)
self.L4_ = Polygon(vertices, transform)
self.joint1_ = RobotRevoluteJoint((0, h), self.jointAngles_[0],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joint2_ = RobotRevoluteJoint((0, 2*h), self.jointAngles_[1],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joint3_ = RobotRevoluteJoint((0, 3*h), self.jointAngles_[2],
-.5*math.pi, .5*math.pi, 1000, False, True)
self.joints_ = [self.joint1_, self.joint2_, self.joint3_]
self.gripper_ = RobotGripper(self.transform_, h, w)
def addToWorld(self, world):
L1 = self.L1_.addToWorld(world, dynamic=False)
L2 = self.L2_.addToWorld(world)
L3 = self.L3_.addToWorld(world)
L4 = self.L4_.addToWorld(world)
joint1 = self.joint1_.addToWorld(world, self.L1_, self.L2_)
joint2 = self.joint2_.addToWorld(world, self.L2_, self.L3_)
joint3 = self.joint3_.addToWorld(world, self.L3_, self.L4_)
gripper = self.gripper_.addToWorld(world)
self.wj_ = world.CreateWeldJoint(
bodyA=L4,
bodyB=self.gripper_.getPalm(),
anchor=self.gripper_.getPalm().position
)
self.body_ = world.CreateDynamicBody(
angle=self.transform_.angle,
position=self.transform_.position,
linearDamping=0.1,
angularDamping=0.1
)
return self.body_
def update(self):
self.joint1_.update()
self.joint2_.update()
self.joint3_.update()
self.gripper_.update()
def getInvKin(self, xy):
if xy[0] < 0.0:
self.jointAngles_ = [0.0, math.pi/3.0, math.pi/3.0]
else:
self.jointAngles_ = [0.0, -math.pi/3.0, -math.pi/3.0]
def distanceToDefault(angles, *args):
sum = np.sum([math.fabs(math.acos(np.dot(np.array([-math.sin(a_i), math.cos(a_i)]),
np.array([-math.sin(d_i), math.cos(d_i)])))*.00001)
for a_i, d_i in zip(angles, self.jointAngles_)])
return sum
def findEndEffector(angles, xy):
start = np.matrix([[0.0],[0.0],[1.0]])
theta = self.L1_.getAngle()
position = self.L1_.getPosition()
T_w = np.matrix([[math.cos(theta),-math.sin(theta),position[0]],
[math.sin(theta),math.cos(theta),self.h],
[0,0,1.0]])
#print(str(angles*180/math.pi))
endEffector = T_w * self.joint1_.getParameterMatrix(angles[0])*\
self.joint2_.getParameterMatrix(angles[1])*\
(self.joint3_.getParameterMatrix(angles[2])+\
self.gripper_.getParameterMatrix(angles[2]))* start
#print(str(endEffector))
s = np.abs(np.array([endEffector[0,0] - xy[0], endEffector[1,0] - xy[1]]))
#print(str(s))
return s
return scipy.optimize.fmin_slsqp(func=distanceToDefault, x0=self.jointAngles_,
f_eqcons=findEndEffector, args=(xy,), iprint=0,
bounds=[(-math.pi,math.pi),(-math.pi,math.pi),
(-math.pi,math.pi)])
def changeOrientation(self, angle):
self.jointAngles_[-1] += angle
def setTargetAngles(self, target_angles):
self.jointAngles_ = target_angles
for i in range(len(target_angles)):
self.joints_[i].setTargetAngle(target_angles[i])
def getTargetAngles(self):
return np.array(self.jointAngles_)
def getGripper(self):
return self.gripper_
def getCurrentAngles(self):
currAngles = [joint.getAngle() for joint in self.joints_]
return currAngles
def ready(self):
return set(round(self.jointAngles_, 2)) == set(round(self.getCurrentAngles, 2))
