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