def calcularFitness(individual, qObjs): fitness = 0 KP = np.array(individual[0:7]) KD = np.array(individual[7:14]) KI = np.array(individual[14:21]) m = { 'left_w0': 0.0, 'left_w1': 0.0, 'left_w2': 0.0, 'left_e0': 0.0, 'left_e1': 0.0, 'left_s0': -1.0, 'left_s1': 0.0 } limb.move_to_joint_positions(m) #Creamos MENSAJE msg = JointCommand() #Modo - 1 (CONTROL POR POSICION) msg.mode = 3 msg.names = [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ] errores = [] errores2 = [] dt = [] t = 0 i = 0 q_sensores = [] #kinematics = baxter_kinematics('left') while i < len(qObjs): dt.append(time.clock()) qDes = qObjs[i] angles = limb.joint_angles() q = np.array([ angles['left_s0'], angles['left_s1'], angles['left_e0'], angles['left_e1'], angles['left_w0'], angles['left_w1'], angles['left_w2'] ]) q_sensores.append(q) error = np.array(qDes - q) errores.append(error) f_ext = np.zeros((6, 1)) f_ext[2] = -9.8 if i != 0: der = (errores[len(errores) - 1] - errores[len(errores) - 2]) / ( dt[len(dt) - 2] - dt[len(dt) - 1]) torque = KP * error + KD * np.array(der) else: der = errores[len(errores) - 1] torque = KP * error + KD * np.array(der) #f = applyFext(kinematics,f_ext) #torque = torque + f print(torque[0]) m = { 'left_w0': torque[4], 'left_w1': torque[5], 'left_w2': torque[6], 'left_e0': torque[2], 'left_e1': torque[3], 'left_s0': torque[0], 'left_s1': torque[1] } msg.command = [ torque[0], torque[1], torque[2], torque[3], torque[4], torque[5], torque[6] ] pub.publish(msg) #Publicamos el mensaje rate.sleep() #Esperamos para conseguir el rate deseado for l in range(len(error)): fitness = fitness + error[l] * error[l] i = i + 10 print(fitness) return fitness
'/ExternalTools/right/PositionKinematicsNode/IKService') ikr = rospy.ServiceProxy( '/ExternalTools/right/PositionKinematicsNode/IKService', SolvePositionIK) ikl = rospy.ServiceProxy( '/ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) pubr = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=1) publ = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) cmd = JointCommand() cmd.mode = 1 req = SolvePositionIKRequest() req.seed_mode = 0 M0 = transformations.quaternion_matrix((0.031, 0.808, .587, 0.028)) M0[0, 3] = 0.7 M0[1, 3] = -0.137 M0[2, 3] = 0.357 Md = np.eye(4) Mrl = np.zeros((4, 4)) Mrl[2, 3] = 0.2 Mrl[2, 0] = Mrl[0, 2] = -1
def process(self): if self.currentAssemblyState == None: #not enabled, return print "Waiting for /robot/state..." time.sleep(1.0) return if self.currentAssemblyState.stopped: print "Robot is stopped" time.sleep(1.0) return if self.currentJointStates == None: print "Waiting for joint states to be published..." time.sleep(1.0) return if self.lastUpdateTime == None: self.lastUpdateTime = self.currentJointStates.header.stamp return self.currentTime = self.currentJointStates.header.stamp #convert to Klamp't message klamptInput = dict() klamptInput['t'] = self.currentTime.to_sec() klamptInput['dt'] = self.currentTime.to_sec( ) - self.lastUpdateTime.to_sec() if klamptInput['dt'] == 0.0: #no new clock messages read return klamptInput['q'] = [0.0] * self.robot_model.numLinks() klamptInput['dq'] = [0.0] * self.robot_model.numLinks() klamptInput['torque'] = [0.0] * self.robot_model.numDrivers() for i, n in enumerate(self.currentJointStates.name): if n not in self.nameToLinkIndex: if n != 'torso_t0': print "Ignoring state of link", n continue klamptIndex = self.nameToLinkIndex[n] klamptInput['q'][klamptIndex] = self.currentJointStates.position[i] if len(self.currentJointStates.velocity) > 0: klamptInput['dq'][ klamptIndex] = self.currentJointStates.velocity[i] if len(self.currentJointStates.effort) > 0: driverIndex = self.nameToDriverIndex[n] klamptInput['torque'][ driverIndex] = self.currentJointStates.effort[i] #if self.currentHeadState != None: # klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan #output is defined in SerialController klamptReply = self.output(**klamptInput) if klamptReply == None: return False #now conver the Klamp't reply to a Baxter command lcmd = JointCommand() rcmd = JointCommand() lcmd.names = self.baxter_larm_names rcmd.names = self.baxter_rarm_names hpcmd = HeadPanCommand() hncmd = Bool() if 'qcmd' in klamptReply: #use position mode lcmd.mode = rcmd.mode = POSITION_MODE q = klamptReply['qcmd'] lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names] rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names] hpcmd.target = q[self.head_pan_link_index] hpcmd.speed = q[self.head_pan_link_index] elif 'dqcmd' in klamptReply: lcmd.mode = rcmd.mode = VELOCITY_MODE dq = klamptReply['dqcmd'] lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names] rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names] hpcmd = None elif 'torquecmd' in klamptReply: lcmd.mode = rcmd.mode = TORQUE_MODE torque = klamptReply['torquecmd'] lcmd.command = [ torque[self.nameToDriverIndex[n]] for n in lcmd.names ] rcmd.command = [ torque[self.nameToDriverIndex[n]] for n in rcmd.names ] hpcmd = None else: lcmd = rcmd = None hpcmd = None hncmd = None if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED: print "Estop is on..." time.sleep(1.0) elif not self.currentAssemblyState.enabled: print "Waiting for robot to turn on..." time.sleep(1.0) else: #publish the messages if lcmd: self.pub_larm.publish(lcmd) if rcmd: self.pub_rarm.publish(rcmd) if hpcmd: self.pub_hpan.publish(hpcmd) if hncmd: self.pub_hnod.publish(hncmd) self.lastUpdateTime = self.currentTime return True
def main(): rospy.init_node('trayectoria_circular') #Ponemos RATE de mensaje rate = rospy.Rate(5) #Creamos objeto PUBLICADOR pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default="left", choices=['left', 'right'], help="joint trajectory limb") parser.add_argument("-R", "--radius", dest="radius", default=0.2, type=float, help="trajectory radius") parser.add_argument("-v", "--velocity", dest="velocity", default=5.0, type=float, help="trajectory velocity") parser.add_argument("-r", "--revolution", dest="revolution", default=0.9, type=float, help="Revolutions") args = parser.parse_args(rospy.myargv()[1:]) kin = baxter_kinematics(args.limb) limb = baxter_interface.Limb(args.limb) timeout = args.velocity / 10.0 radio = args.radius decremento = 0.001 posiciones = [] centro_y = 0.35 centro_z = 0.1 if args.limb == 'right': centro_y = centro_y * (-1) # FK Position limb.move_to_neutral() kp = kin.forward_position_kinematics() #Array donde vamos acumulando configuraciones q1,q2,...,qn para seguir la trayectoria #Util para control por Torques con PID posiciones.append(kp) #Centro de la Circunferencia pos = [kp[0], kp[1] - centro_y, kp[2] + centro_z] rot = [kp[3], kp[4], kp[5], kp[6]] #Calculamos configuracion Qcentro l = kin.inverse_kinematics(pos, rot) m = { args.limb + '_w0': l[4], args.limb + '_w1': l[5], args.limb + '_w2': l[6], args.limb + '_e0': l[2], args.limb + '_e1': l[3], args.limb + '_s0': l[0], args.limb + '_s1': l[1] } #Movemos al centro de la circunferencia limb.move_to_joint_positions(m) posiciones.append(l) puntos_circunferencia = [] puntos_circunferencia2 = [] #Calculamos los valores que tomara Z y que nos servira para calcular Y posteriormente valoresTomar = [] i = 1 while (i >= -0.5): valoresTomar.append(i) i = i - decremento #Calculamos las posiciones para describir circunferencia en plano ZY for z in valoresTomar: c = 2 * pos[1] b = (-1) * (-z * z + 2 * z * pos[2] + radio * radio - pos[2] * pos[2] - pos[1] * pos[1]) if c * c - 4 * b > 0: y = (c + sqrt(c * c - 4 * b)) / 2 y2 = (c - sqrt(c * c - 4 * b)) / 2 puntos_circunferencia.append([y, z]) puntos_circunferencia2.append([y2, z]) Posiciones = [] Rotaciones = [] ErroresPos = [] ErroresRot = [] #Vamos PUBLICANDO las posiciones que deseamos alcanzar para seguir la trayectoria #Creamos MENSAJE msg = JointCommand() #Modo - 1 (CONTROL POR POSICION) msg.mode = 1 msg.names = [ args.limb + '_w0', args.limb + '_w1', args.limb + '_w2', args.limb + '_e0', args.limb + '_e1', args.limb + '_s0', args.limb + '_s1' ] for p in puntos_circunferencia2: kp = kin.forward_position_kinematics() #Calculamos (x,y,z) pos = [kp[0], p[0], p[1]] #Calculamos tupla Orientacion (i,j,w,k) rot = [kp[3], kp[4], kp[5], kp[6]] #Calculamos (theta1,...) para (x,y,z) l = kin.inverse_kinematics(pos, rot) #Posicion Valida if l != None: Posiciones.append(pos) Rotaciones.append(rot) msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]] pub.publish(msg) rate.sleep() ErroresRot.append(rot) posiciones.append(l) else: ErroresPos.append(pos) puntos_circunferencia = puntos_circunferencia[::-1] for p in puntos_circunferencia: kp = kin.forward_position_kinematics() pos = [kp[0], p[0], p[1]] rot = [kp[3], kp[4], kp[5], kp[6]] l = kin.inverse_kinematics(pos, rot) if l != None: Posiciones.append(pos) Rotaciones.append(rot) msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]] pub.publish(msg) rate.sleep() posiciones.append(l) else: ErroresPos.append(pos) ErroresRot.append(rot) #Caso de que queramos realizar la trayectoria en mas de una ocasion n_vueltas = args.revolution k = 1 while k < n_vueltas: for i in range(len(Posiciones)): pos = Posiciones[i] rot = Rotaciones[i] l = kin.inverse_kinematics(pos, rot) msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]] pub.publish(msg) rate.sleep() k = k + 1 np.save('trayectoriaC', np.array(posiciones))
def callback(joints): joints=JointCommand() _joints.mode=joints.mode _joints.names=copy(joints.names) _joints.command=copy(joints.command)
def __init__(self, addr, robot_model, robot_name='robot'): SerialController.__init__(self, addr) #defined in SerialController self.robot_model = robot_model self.baxter_larm_names = [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ] self.baxter_rarm_names = [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] self.names_klampt_to_baxter = { 'left_upper_shoulder': 'left_s0', 'left_lower_shoulder': 'left_s1', 'left_upper_elbow': 'left_e0', 'left_lower_elbow': 'left_e1', 'left_upper_forearm': 'left_w0', 'left_lower_forearm': 'left_w1', 'left_wrist': 'left_w2', 'right_upper_shoulder': 'right_s0', 'right_lower_shoulder': 'right_s1', 'right_upper_elbow': 'right_e0', 'right_lower_elbow': 'right_e1', 'right_upper_forearm': 'right_w0', 'right_lower_forearm': 'right_w1', 'right_wrist': 'right_w2', 'head': 'head_pan', 'screen': 'head_nod' } for l in range(self.robot_model.numLinks()): klamptName = self.robot_model.link(l).getName() if klamptName not in self.names_klampt_to_baxter: self.names_klampt_to_baxter[klamptName] = klamptName self.baxterDriverNames = [ self.names_klampt_to_baxter[self.robot_model.getDriver( d).getName()] for d in range(self.robot_model.numDrivers()) ] self.head_pan_link_index = self.robot_model.link("head").index self.head_nod_link_index = self.robot_model.link("screen").index self.larm_command = JointCommand() self.rarm_command = JointCommand() self.hpan_command = HeadPanCommand() self.hnod_command = Bool() # fast indexing structure for partial commands self.nameToDriverIndex = dict( zip(self.baxterDriverNames, range(len(self.baxterDriverNames)))) self.nameToLinkIndex = dict( (self.names_klampt_to_baxter[self.robot_model.link(l).getName()], l) for l in range(self.robot_model.numLinks())) # Setup publisher of robot states self.currentTime = None self.lastUpdateTime = None self.currentAssemblyState = None self.currentJointStates = None self.currentHeadState = None rospy.Subscriber("/%s/state" % (robot_name, ), AssemblyState, self.assemblyStateCallback) rospy.Subscriber("/%s/joint_states" % (robot_name, ), JointState, self.jointStatesCallback) rospy.Subscriber("/%s/head/head_state" % (robot_name, ), HeadState, self.headStateCallback) self.pub_larm = rospy.Publisher( '/%s/limb/left/joint_command' % (robot_name), JointCommand) self.pub_rarm = rospy.Publisher( '/%s/limb/right/joint_command' % (robot_name), JointCommand) self.pub_hpan = rospy.Publisher( '/%s/head/command_head_pan' % (robot_name), HeadPanCommand) self.pub_hnod = rospy.Publisher( '/%s/head/command_head_nod' % (robot_name), Bool) return
def main(): rospy.init_node('trayectoria_rombo') #Definimos el RATE de publicacion de mensajes en ROS rate = rospy.Rate(20) #Creamos objeto PUBLICADOR pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument("-l", "--limb", dest="limb", default="left", choices=['left', 'right'], help="joint trajectory limb") parser.add_argument("-v", "--velocity", dest="velocity", default=5.0, type=float, help="trajectory velocity") #Leemos los argumentos dados como entrada args = parser.parse_args(rospy.myargv()[1:]) kin = baxter_kinematics(args.limb) limb = baxter_interface.Limb(args.limb) timeout = args.velocity / 10.0 n_mov = 32 # Movemos a posicion neutral limb.move_to_neutral() # Consultamos la posicion actual en formato (x,y,z) y (i,j,w,k) kp = kin.forward_position_kinematics() #Limites del rombo a describir limite_inferior_y = -0.5 limite_inferior_z = -0.5 proporcionY = limite_inferior_y / n_mov proporcionZ = limite_inferior_z / n_mov if args.limb == 'right': proporcionY = proporcionY * (-1) #Creamos MENSAJE msg = JointCommand() #Modo - 1 (CONTROL POR POSICION) msg.mode = 1 msg.names = [ args.limb + '_w0', args.limb + '_w1', args.limb + '_w2', args.limb + '_e0', args.limb + '_e1', args.limb + '_s0', args.limb + '_s1' ] #Vamos describiendo la trayectoria de Rombo for i in range(n_mov): kp = kin.forward_position_kinematics() if i < n_mov // 2: pos = [kp[0], kp[1] + proporcionY, kp[2] - proporcionZ] else: pos = [kp[0], kp[1] + proporcionY, kp[2] + proporcionZ] rot = [kp[3], kp[4], kp[5], kp[6]] theta_i = kin.inverse_kinematics(pos, rot) if theta_i != None: msg.command = [ theta_i[4], theta_i[5], theta_i[6], theta_i[2], theta_i[3], theta_i[0], theta_i[1] ] pub.publish(msg) #Publicamos el mensaje rate.sleep() #Esperamos para conseguir el rate deseado for i in range(n_mov): kp = kin.forward_position_kinematics() if i < n_mov // 2: pos = [kp[0], kp[1] - proporcionY, kp[2] + proporcionZ] else: pos = [kp[0], kp[1] - proporcionY, kp[2] - proporcionZ] rot = [kp[3], kp[4], kp[5], kp[6]] theta_i = kin.inverse_kinematics(pos, rot) if theta_i != None: msg.command = [ theta_i[4], theta_i[5], theta_i[6], theta_i[2], theta_i[3], theta_i[0], theta_i[1] ] pub.publish(msg) rate.sleep()
## Flags ## Recommended (stable) parameters are: PY_KDL and MOVE_JNT_PSTN reference_pose_flag = 1 # Used to determine whether to used saved joint angle data for the reference position (true) or not (false). #Inverse Kinematics Computation PY_KDL=0 TRAC_IK=1 kinematics_flag=PY_KDL # Used to determine wheter to use kdl or trac_ik for kinematics # Movement Algorithm MOVE_JNT_PSTN=0 JOINT_ACT_CLN=1 approach_flag=MOVE_JNT_PSTN # Used to determine whether to use move_to_joint_positions or joint_trajectory_action_client approach # Globals _joints = JointCommand() #------------------------------------ Class ___________ ------------------------------ class Trajectory(object): def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( # create simple client w/ topic and msg type ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() # trajectory(header/points); # path_tolerance # goal_tolerance # goal_time_tolerance self._goal_time_tolerance = rospy.Time(0.1) # Reach goal within this tolerance of final time. self._goal.goal_time_tolerance = self._goal_time_tolerance # Assign to goal object. server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) # Connect to server within this time.
def main(): rospy.init_node('trayectoria_sinusoidal') #Ponemos RATE de mensaje rate = rospy.Rate(1) #Creamos objeto PUBLICADOR pub = rospy.Publisher('/robot/limb/left/joint_command',JointCommand,queue_size=10) arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument( "-l", "--limb", dest="limb", default="left", choices=['left', 'right'], help="joint trajectory limb" ) parser.add_argument( "-v", "--velocity", dest="velocity", default=5.0, type=float, help="trajectory velocity" ) parser.add_argument( "-r", "--revolution", dest="revolution", default=10.0, type=float, help="Revolutions" ) parser.add_argument( "-t", "--dt", dest="dt", default=0.025, type=float, help="Differential T" ) parser.add_argument( "-n", "--points", dest="n", default=100, type=int, help="Number of movements" ) args = parser.parse_args(rospy.myargv()[1:]) kin = baxter_kinematics(args.limb) limb = baxter_interface.Limb(args.limb) timeout = args.velocity/10.0 n_mov = args.n dt = args.dt t = 0 #Movemos a posicion neutral limb.move_to_neutral(); #Creamos MENSAJE msg = JointCommand() #Modo - 1 (CONTROL POR POSICION) msg.mode = 1 msg.names = [args.limb +'_w0',args.limb + '_w1',args.limb +'_w2',args.limb +'_e0',args.limb +'_e1',args.limb +'_s0',args.limb +'_s1'] #Realizamos trayectoria sinusoidal contador = 0; while(contador < n_mov): kp = kin.forward_position_kinematics() x = 0.5 y = t/100 z = 0.1*sin(3*t) if(args.limb == 'right'): y = y*(-1); pos = [kp[0],kp[1]-y,kp[2]-z] rot = [kp[3],kp[4],kp[5],kp[6]] theta_i = kin.inverse_kinematics(pos,rot); if theta_i != None: msg.command = [theta_i[4],theta_i[5],theta_i[6],theta_i[2],theta_i[3],theta_i[0],theta_i[1]] pub.publish(msg) #Publicamos el mensaje rate.sleep() #Esperamos para conseguir el rate deseado t = t + dt contador = contador + 1;
def __init__(self): # Initialize main node for publishing rospy.init_node('MainAction') rospy.loginfo('... Initializing MainAction node') # Enable baxter if it isn't already enabled baxter_interface.RobotEnable(CHECK_VERSION).state().enabled baxter_interface.RobotEnable(CHECK_VERSION).enable() # Get create main publisher for activating the Joint movements self.pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) # Create Class to control Baxter's Joints self.command = JointCommand() self.rate = rospy.Rate(70) # Variable to show every print statement for testing SHOW_RESULTS = False # Define screen pixels for Baxter's image (face LCD screen) h = 600 w = 1024 # Load image for the Baxter's screen img1 = cv2.imread("Tejada.png") # Resize the image to get to the maximum possible screen size img1 = cv2.resize(img1, (w, h), interpolation=cv2.INTER_CUBIC) # Create publisher for the Image that will be loaded to Baxter pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) # Define message that will be loaded to the Image-publisher msg1 = cv_bridge.CvBridge().cv2_to_imgmsg(img1, encoding="bgr8") # Publish the desired message that contains the image pub.publish(msg1) # Get main vector for the trajectory T1 = trajectory_A.TrajectoryA(5) print("\n... Processing trajectory 1 ...\n") vector_x = T1.vector_x vector_y = T1.vector_y vector_z = T1.vector_z vector_angle_x = T1.vector_angle_x vector_angle_y = T1.vector_angle_y vector_angle_z = T1.vector_angle_z print("\n... Done processing trajectory 1 ...\n") # TM_test = transformation.Transformation(vector_angle_x[0], vector_angle_y[0], # vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]]) # print(TM_test.TM) # THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left") # print(THETAS) # Get each vector of theta_i for all points self.THETA_1 = [] self.THETA_2 = [] self.THETA_4 = [] self.THETA_5 = [] self.THETA_6 = [] self.THETA_7 = [] # Generate main trajectories finding each angle of the joints with IK for i in range(len(vector_x)): # Get each transformation matrix for all cartesian points given TM_i = transformation.Transformation( vector_angle_x[i], vector_angle_y[i], vector_angle_z[i], [vector_x[i], vector_y[i], vector_z[i]]) # Get each THETA_i for each point with the inverse-kinematics THETAS_i = inverse_kinematics.inverse_kinematics_baxter( TM_i.TM, "left") # Create all vectors of THETA_i for all necessary points self.THETA_1.append(float(THETAS_i[0])) self.THETA_2.append(float(THETAS_i[1])) self.THETA_4.append(float(THETAS_i[2])) self.THETA_5.append(float(THETAS_i[3])) self.THETA_6.append(float(THETAS_i[4])) self.THETA_7.append(float(THETAS_i[5])) if SHOW_RESULTS == True: print("\n THETA_1: \n", np.rad2deg(self.THETA_1)) print("\n max(THETA_1) =", max(np.rad2deg(self.THETA_1))) print("\n min(THETA_1) =", min(np.rad2deg(self.THETA_1))) print("\n THETA_2: \n", np.rad2deg(self.THETA_2)) print("\n max(THETA_2) =", max(np.rad2deg(self.THETA_2))) print("\n min(THETA_2) =", min(np.rad2deg(self.THETA_2))) print("\n THETA_4: \n", np.rad2deg(self.THETA_4)) print("\n max(THETA_4) =", max(np.rad2deg(self.THETA_4))) print("\n min(THETA_4) =", min(np.rad2deg(self.THETA_4))) print("\n THETA_5: \n", np.rad2deg(self.THETA_5)) print("\n max(THETA_5) =", max(np.rad2deg(self.THETA_5))) print("\n min(THETA_5) =", min(np.rad2deg(self.THETA_5))) print("\n THETA_6: \n", np.rad2deg(self.THETA_6)) print("\n max(THETA_6) =", max(np.rad2deg(self.THETA_6))) print("\n min(THETA_6) =", min(np.rad2deg(self.THETA_6))) print("\n THETA_7: \n", np.rad2deg(self.THETA_7)) print("\n max(THETA_7) =", max(np.rad2deg(self.THETA_7))) print("\n min(THETA_7) =", min(np.rad2deg(self.THETA_7)))
def __init__(self, limb): """ Constructor. @type limb: str @param limb: limb to interface """ self.name = limb self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._joint_names = { 'left': [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ], 'right': [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] } ns = '/robot/limb/' + limb + '/' self._command_msg = JointCommand() self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio', Float64, latch=True, queue_size=10) self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command', JointCommand, tcp_nodelay=True, queue_size=1) self._pub_joint_cmd_timeout = rospy.Publisher(ns + 'joint_command_timeout', Float64, latch=True, queue_size=10) _cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state', EndpointState, self._on_endpoint_states, queue_size=1, tcp_nodelay=True) joint_state_topic = 'robot/joint_states' _joint_state_sub = rospy.Subscriber(joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) err_msg = ("%s limb init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg)
#!/usr/bin/env python from numpy import * import rospy from baxter_core_msgs.msg import JointCommand pub_joints = rospy.Publisher('/robot/limb/right/joint_command', JointCommand) rospy.init_node('write_to_baxr', anonymous=False) rate = rospy.Rate(20) cmd_msg = JointCommand() cmd_msg.mode = JointCommand.POSITION_MODE cmd_msg.names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] qini= [0.14035924209151535, -0.9794467330648366,1.588437105855346,0.39154859610775183,-0.8233641878974959, 0.7113835903818606, 0.8348690438066364] while not rospy.is_shutdown(): cmd_msg.command = qini pub_joints.publish(cmd_msg) rate.sleep()
class MovementController(object): def __init__(self, limb, topic,camera,speedRatio, jointThresholdEnd, jointThresholdWarning, updateQueryRate, jointFilteringFactorFar, jointFilteringFactorClose, orientationX, orientationY, orientationZ, orientationW, timeToWait, name): # Set Confiuration Variables self.limb = limb self.topic=topic self.camera = camera self.speedRatio=speedRatio self.jointThresholdEnd=jointThresholdEnd self.jointThresholdWarning=jointThresholdWarning self.updateQueryRate=updateQueryRate self.jointFilteringFactorFar=jointFilteringFactorFar self.jointFilteringFactorClose=jointFilteringFactorClose self.orientationX=orientationX self.orientationY=orientationY self.orientationZ=orientationZ self.orientationW=orientationW self.timeToWait=timeToWait # Set variables that are static across executions of the actionServer self.handDetector = self.createHandDetector() self.pub_joint_cmd = rospy.Publisher( '/robot/limb/' + self.limb + '/joint_command', JointCommand, tcp_nodelay=True, queue_size=1) self.pub_speed_ratio = rospy.Publisher( '/robot/limb/' + self.limb + '/set_speed_ratio', Float64, latch=True, queue_size=1) self.pub_speed_ratio.publish(Float64(speedRatio)) # Limit arm speed self.jointAngles = dict() self.jointAnglesLock = threading.Lock() joint_state_topic = 'robot/joint_states' self.joint_state_sub = rospy.Subscriber( joint_state_topic, JointState, self.onJointStates, queue_size=1, tcp_nodelay=True) self.iksvcStringLimb = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self.iksvcLimb = rospy.ServiceProxy(self.iksvcStringLimb, SolvePositionIK) # Set Action Server-Related Variables self.feedback = ReachingHandFeedback() self.result = ReachingHandResult() self.actionServer = actionlib.SimpleActionServer(name, ReachingHandAction, execute_cb=self.execute_cb, auto_start = False) self.actionServer.start() def createHandDetector(self): detector = HandDetector( topic=rospy.get_param("reachingHand/topic"), rate=rospy.get_param("reachingHand/HandDetector/rate"), cameraName=self.camera, handModelPath=rospy.get_param("reachingHand/HandDetector/handModelPath"), maxDx=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dx"), maxDy=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dy"), maxDz=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dz"), timeToDeleteHand=rospy.get_param("reachingHand/HandDetector/timeToDeleteHand"), groundDzThreshold=rospy.get_param("reachingHand/HandDetector/groundDzThreshold"), avgPosDtime=rospy.get_param("reachingHand/HandDetector/avgPosDtime"), avgHandXYZDtime=rospy.get_param("reachingHand/HandDetector/avgHandXYZDtime"), maxIterationsWithNoDifference=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/maxIterations"), differenceThreshold=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/differenceThreshold"), differenceFactor=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/differenceFactor"), cascadeScaleFactor=rospy.get_param("reachingHand/HandDetector/cascadeClassifierParams/scale"), cascadeMinNeighbors=rospy.get_param("reachingHand/HandDetector/cascadeClassifierParams/minNeighbors"), handHeightIntervalDx=rospy.get_param("reachingHand/HandDetector/handHeightInterval/dx"), handHeightIntervalDy=rospy.get_param("reachingHand/HandDetector/handHeightInterval/dy"), getDepthAtMidpointOfHand=rospy.get_param("reachingHand/HandDetector/getDepthAtMidpointOfHand"), getAveragePos=rospy.get_param("reachingHand/HandDetector/getAveragePos"), ) return detector def execute_cb(self, goal): # Launch the HandDetector self.handDetector.start() # Subscribe to necessary topics and create necessary background threads self.targetPose = None self.shouldUpdateBaxterTarget = False self.shouldUpdateBaxterTargetLock = threading.Lock() self.targetPosition = dict() self.targetPoint = None self.killThreads = False self.targetPositionLock = threading.Lock() self.targetEndEffectorPointTopic = rospy.Subscriber(self.topic, PointStamped, self.targetEndEffectorPointCallback, queue_size=1) self.targetEndEffectorPoint = None self.targetEndEffectorPointLock = threading.Lock() endEffectorTopic = "robot/limb/"+self.limb+"/endpoint_state" self.actualEndEffectorPoseTopic = rospy.Subscriber(endEffectorTopic, EndpointState, self.actualEndEffectorPoseCallback, queue_size=1) self.actualEndEffectorPose = None self.actualEndEffectorPoseLock = threading.Lock() endEffectorThread = threading.Thread(target=self.setTargetEndEffectorPosition, args=(self.orientationX, self.orientationY, self.orientationZ, self.orientationW)) endEffectorThread.daemon = True endEffectorThread.start() # Move the arm self.moveArm(self.jointThresholdEnd, self.jointThresholdWarning, self.updateQueryRate, self.jointFilteringFactorFar, self.jointFilteringFactorClose) # Kill threads, unregister topics, and kill the HandDetector Node self.killThreads = True self.targetEndEffectorPointTopic.unregister() self.actualEndEffectorPoseTopic.unregister() self.handDetector.stop() def onJointStates(self, msg): for i, name in enumerate(msg.name): # print("onJointStates", i, name) if self.limb in name: self.jointAnglesLock.acquire() self.jointAngles[name] = msg.position[i] self.jointAnglesLock.release() def actualEndEffectorPoseCallback(self, state): if self.actualEndEffectorPoseLock.acquire(False): self.actualEndEffectorPose = state.pose self.actualEndEffectorPoseLock.release() def moveArm(self, jointThresholdEnd, jointThresholdWarning, updateQueryRate, jointFilteringFactorFar, jointFilteringFactorClose): rate = rospy.Rate(updateQueryRate) print("moveArm") def genf(targetPoint, targetOrientation): def endEffectorDiff(): self.actualEndEffectorPoseLock.acquire() if self.actualEndEffectorPose is None: retValPos = 0 retValOrien = 0 else: retValPos = math.sqrt((self.actualEndEffectorPose.position.x-targetPoint.x)**2+ (self.actualEndEffectorPose.position.y-targetPoint.y)**2+ (self.actualEndEffectorPose.position.z-targetPoint.z)**2) retValOrien = math.sqrt((self.actualEndEffectorPose.orientation.x-targetOrientation.x)**2+ (self.actualEndEffectorPose.orientation.y-targetOrientation.y)**2+ (self.actualEndEffectorPose.orientation.z-targetOrientation.z)**2+ (self.actualEndEffectorPose.orientation.w-targetOrientation.w)**2) # print(joint, retVal) self.actualEndEffectorPoseLock.release() return retValPos, retValOrien return endEffectorDiff # Wait until we have read the joint state at least once try: while not rospy.is_shutdown() and (len(self.jointAngles) == 0 or len(self.targetPosition) == 0 or self.actualEndEffectorPose is None) and not self.killThreads: if self.actionServer.is_preempt_requested(): self.preempt() return rate.sleep() continue except KeyboardInterrupt, rospy.ROSInterruptException: self.abort() return reachedTarget = True commandMsg = JointCommand() try: while not rospy.is_shutdown() and not self.killThreads: if self.actionServer.is_preempt_requested(): self.preempt() return if reachedTarget: self.shouldUpdateBaxterTargetLock.acquire() if not self.shouldUpdateBaxterTarget: self.shouldUpdateBaxterTargetLock.release() rate.sleep() continue self.shouldUpdateBaxterTargetLock.release() # TODO (amal): look into exponential moving average def filtered_cmd(): # print(self.targetPosition, self.jointAngles) if not self.closeToTarget: self.targetPositionLock.acquire() retPositions = dict() factor =jointFilteringFactorFar if self.closeToTarget: factor = jointFilteringFactorClose for joint in self.targetPosition.keys(): if self.currentlyMovingTowardsPoint is None or not self.closeToTarget: self.currentlyMovingTowardsPoint = self.targetPosition retPositions[joint] = factor * self.currentlyMovingTowardsPoint[joint] + (1-factor) * self.jointAngles[joint] # Get most up to date value if not self.closeToTarget: self.targetPositionLock.release() return retPositions self.closeToTarget = False self.currentlyMovingTowardsPoint = None def loopGuard(): if rospy.is_shutdown() or self.killThreads: return False allDiffsWithinWarning = True # TODO (amal): is this a race condition because I don't lock targetPose? positionDiff, orientationDiff = genf(self.targetPose.pose.position, self.targetPose.pose.orientation)() diff = positionDiff+orientationDiff self.feedback.distance = float(diff) self.actionServer.publish_feedback(self.feedback) if diff > jointThresholdWarning: allDiffsWithinWarning = False if allDiffsWithinWarning and not self.closeToTarget: print("WITHIN WARNING!") # Don't let the target point get changed from now till the hand reaches its target self.targetPositionLock.acquire() print("acquired target position lock") self.closeToTarget = True positionDiff, orientationDiff = genf(self.targetPose.pose.position, self.targetPose.pose.orientation)() if positionDiff+orientationDiff > jointThresholdEnd: print("outside of threshold") return True return False try: # print("before loop guard") while loopGuard(): # print("loop guard true") position = filtered_cmd() commandMsg.names = position.keys() commandMsg.command = position.values() # print(self.jointAngles, self.targetPosition, position) commandMsg.mode = JointCommand.POSITION_MODE self.pub_joint_cmd.publish(commandMsg) print("pre end movememnt", self.closeToTarget) except KeyboardInterrupt, rospy.ROSInterruptException: if self.closeToTarget: self.targetPositionLock.release() self.closeToTarget = False self.abort() return print("ended movememnt", self.closeToTarget) if self.closeToTarget: self.targetPositionLock.release() self.closeToTarget = False reachedTarget = True self.shouldUpdateBaxterTargetLock.acquire() self.shouldUpdateBaxterTarget = False print("DONE DONE DONE!!!") self.shouldUpdateBaxterTargetLock.release() self.success() return except KeyboardInterrupt, rospy.ROSInterruptException: self.abort() return
def moveVelocity(self, joint_values_target, speed, thresh, \ forceThresh=float('inf'), maxAcceleration=0.2, is_printing=False): ''' Move the arm using velocity control to a given list of joint positions. @type jointTarget: 1x7 vector @param jointTarget: the given joint configuration @type speed: real number @param speed: the speed at which the arm should move @type thresh: real number @param thresh: the threshold at which the arm is close enough to the desired joint positions @type is_printing: bool @param is_printing: true if debug information should be printed, false otherwise ''' if not self.isMoving: return max_iterations = 10000 step_size = 2.0 oldVelocityCommand = array(self.joint_velocities) for i in range(0, max_iterations): joint_error = array(self.joint_values) joint_velocity_command = joint_error # calculate joint error, error magnitude, and command velocity joint_error = joint_values_target - joint_error joint_velocity_command = joint_error * step_size error_magnitude = numpy.linalg.norm(joint_error) joint_velocity_command_magnitude = error_magnitude * step_size # if command velocity magnitude exceeds speed, scale it back to speed if joint_velocity_command_magnitude > speed: joint_velocity_command = speed * joint_velocity_command / joint_velocity_command_magnitude # scale back if over acceleration limits acceleration = norm(joint_velocity_command - oldVelocityCommand) if acceleration > maxAcceleration: #print("Exceeded max acceleration: {}.".format(acceleration)) a = (maxAcceleration / acceleration) joint_velocity_command = a * joint_velocity_command + ( 1.0 - a) * oldVelocityCommand oldVelocityCommand = joint_velocity_command # exit if force magnitude exceeded if self.forceMagnitude > forceThresh: print("Force is {}, exceeded threshold {}. Stopping.".format( self.forceMagnitude, forceThresh)) break # send velocity command to joints values_out = [ joint_velocity_command[2], joint_velocity_command[3], joint_velocity_command[0], joint_velocity_command[1], joint_velocity_command[4], joint_velocity_command[5], joint_velocity_command[6] ] self.pub.publish( JointCommand(JointCommand.VELOCITY_MODE, values_out, self.joint_names)) if is_printing: print "iteration:", i, "error_magnitude: ", error_magnitude # break loop if close enough to target joint values if error_magnitude < thresh: break rospy.sleep(0.01)
def __init__(self, limb): """mZ Constructor. @type limb: str @param limb: limb to interface """ self.name = limb self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self.start = False self.jt = JointTransform() self.force_cali_mode = True self._joint_names = { 'left': [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ], 'right': [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] } ns = '/robot/limb/' + limb + '/' self.record = open('force.txt', 'w') self._command_msg = JointCommand() self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio', Float64, latch=True, queue_size=10) self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command', JointCommand, tcp_nodelay=True, queue_size=1) self._pub_joint_cmd_timeout = rospy.Publisher(ns + 'joint_command_timeout', Float64, latch=True, queue_size=10) self.x = [] self.y = [] self.step = 0 self.calibrate = False self.calibrated = False self.base_force_list = [] self.force_window = [] self.force_thrd = 1.6 _cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state', EndpointState, self._on_endpoint_states, queue_size=20, tcp_nodelay=True) joint_state_topic = 'robot/joint_states' _joint_state_sub = rospy.Subscriber(joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) err_msg = ("%s limb init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg) err_msg = ("%s limb init failed to get current endpoint_state " "from %s") % (self.name.capitalize(), ns + 'endpoint_state') baxter_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0, timeout_msg=err_msg)