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))