def getpose(): pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand) command_msg=JointCommand() command_msg.names=['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] command_msg.mode=JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() joint_positions=rospy.wait_for_message("/robot/joint_states",JointState) qc = (joint_positions.position[9:16]) angles = [qc[2],qc[3],qc[0],qc[1],qc[4],qc[5],qc[6]] pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand) command_msg=JointCommand() command_msg.names=['right_e1'] command_msg.command=[0] command_msg.mode=JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() exit = 0 listener2=tf.TransformListener() #the transformations now=rospy.Time() listener2.waitForTransform("/torso","/right_hand",now,rospy.Duration(1.0)) (trans08,rot08)=listener2.lookupTransform("/torso","/right_hand",now) # Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1]) R08 = transformations.quaternion_matrix(rot08) T08 = numpy.vstack((numpy.column_stack((R08[0:3,0:3], numpy.transpose(numpy.array(trans08)))),[0,0,0,1])) return (angles, T08)
def send_to_joint_vals(q, arm='right'): pub_joint_cmd = rospy.Publisher('/robot/limb/' + arm + '/joint_command', JointCommand) command_msg = JointCommand() command_msg.names = generate_joint_names_for_arm(arm) command_msg.command = q command_msg.mode = JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() joint_positions = rospy.wait_for_message("/robot/joint_states", JointState) qc = joint_positions.position[9:16] qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]]) print('q,qc') print(q) print(qc) while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract( q, qc)) > 0.07: pub_joint_cmd.publish( command_msg) # sends the commanded joint values to Baxter control_rate.sleep() joint_positions = rospy.wait_for_message("/robot/joint_states", JointState) qc = (joint_positions.position[9:16]) qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]) print "joint error = ", numpy.linalg.norm(numpy.subtract(q, qc)) print((q, qc)) print("In home pose") return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def send_to_joint_vals(q, arm='right'): pub_joint_cmd=rospy.Publisher('/robot/limb/' + arm + '/joint_command',JointCommand) command_msg=JointCommand() command_msg.names = generate_joint_names_for_arm(arm) command_msg.command=q command_msg.mode=JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() joint_positions=rospy.wait_for_message("/robot/joint_states",JointState) qc = joint_positions.position[9:16] qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]]) print('q,qc') print(q) print(qc) while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(q,qc))> 0.07: pub_joint_cmd.publish(command_msg) # sends the commanded joint values to Baxter control_rate.sleep() joint_positions=rospy.wait_for_message("/robot/joint_states",JointState) qc = (joint_positions.position[9:16]) qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]) print "joint error = ", numpy.linalg.norm(numpy.subtract(q,qc)) print((q,qc)) print("In home pose") return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def main(): rospy.init_node('jointPositionPublisherExample', anonymous=True) pub_joint_cmd = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) # rospy.Subscriber('/robot/limb/' + 'left' +'/gravity_compensation_torques', SEAJointState, force_sensor_callback) limb = baxter_interface.Limb("left") limb.set_joint_position_speed(0.5) limb.set_command_timeout(120.0) cmdMsg = JointCommand() cmdMsg.mode = JointCommand.POSITION_MODE # q untucked, considered to be the initial q: 'left_w0': 0.669996718047412, 'left_w1': 1.030008750129423, 'left_w2': -0.4999996908801263, 'left_e0': -1.1899720096239292, 'left_e1': 1.940029476507764, 'left_s0': -0.08000000085054282, 'left_s1': -0.9999845808672081 q = limb.joint_angles() #joint configuration q['left_w2'] = 3 cmdMsg.command = q.values() cmdMsg.names = q.keys() print(cmdMsg) pub_joint_cmd.publish(cmdMsg) #positions = dict(zip(self.limb.joint_names(), theta)) rate = rospy.Rate(2) while not rospy.is_shutdown(): printCurJointPos(limb) pub_joint_cmd.publish(cmdMsg) rate.sleep()
def move_list_smooth_wobble(neutral=False, arm=None, p_list=0, timeout=default_timeout, threshold=default_threshold, speed=default_speed): arm = arm.lower() if speed > 1.0: speed = 1.0 elif speed <= 0.0: speed = 0.1 if arm == 'l': arm = 'left' elif arm == 'r': arm = 'right' if arm == 'left' or arm == 'right': limb = baxter_interface.Limb(arm) limb.set_joint_position_speed(speed) if neutral: limb.move_to_neutral() for positions in p_list: position = dictToList(limb, positions) pub_joints = rospy.Publisher('/robot/limb/' + arm + '/joint_command', JointCommand, queue_size=5) cmd_msg = JointCommand() cmd_msg.mode = JointCommand.POSITION_MODE cmd_msg.names = [ arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0', arm + '_e1', arm + '_s0', arm + '_s1' ] cmd_msg.names = [ arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0', arm + '_e1', arm + '_s0', arm + '_s1' ] bo_finish = False while not bo_finish and not rospy.is_shutdown(): cmd_msg.command = position pub_joints.publish(cmd_msg) time.sleep(0.19) bo_finish = not checkThreshold(limb, position, threshold) if neutral: limb.move_to_neutral() limb.set_joint_position_speed(default_speed) else: print("Error: {} is not a valid limb".format(arm))
def stopMoving(self): lcmd = JointCommand() rcmd = JointCommand() lcmd.names = self.baxter_larm_names rcmd.names = self.baxter_rarm_names lcmd.mode = rcmd.mode = POSITION_MODE lcmd.command = [] for n in lcmd.names: index = self.currentJointStates.name.index(n) lcmd.command.append(self.currentJointStates.position[index]) rcmd.command = [] for n in rcmd.names: index = self.currentJointStates.name.index(n) rcmd.command.append(self.currentJointStates.position[index]) self.pub_larm.publish(lcmd) self.pub_rarm.publish(rcmd) self.pub_hnod.publish(False)
def runExperiment(Kp, Kd, Ki, commandHz): bc = BaxterCache() print("Resetting robot") tucker = baxter_tools.Tuck(False) rospy.on_shutdown(tucker.clean_shutdown) tucker.supervised_tuck() rospy.sleep(1) controlTopic = "/robot/limb/right/joint_command" pub = rospy.Publisher(controlTopic, JointCommand, queue_size=20) messageObj = JointCommand() messageObj.mode = JointCommand.TORQUE_MODE messageObj.names = joint_names rate = rospy.Rate(commandHz) print("Waiting for first joint_state message") while bc.joints is None: rate.sleep() print("First joint_state message acquired") print("Waiting for first target_loc message") while bc.get_desired_joints_and_vel() is None: rate.sleep() print("First target_loc message acquired") time.sleep(1) cumulativeJointError = np.zeros(bc.joints.shape) fileName = "results/Results%s.csv" % ("Interp" if bc.trackFuture else "Naive") print("logging to %s" % fileName) with StateLogger(fileName) as logger: it = 0 maxIt = 2000 * commandHz while not rospy.is_shutdown(): # and it < maxIt: des = bc.get_desired_joints_and_vel() if des is None or des[0] is None: continue desired_joints = des[0] desired_dot = des[1] # compute the current joint angles err = bc.joints - desired_joints # note this needs to be scaled by freq of messages cumulativeJointError += err / 1000 * 2 err_dot = bc.joints_dot - desired_dot # log at 40 hz if it % int(max(1, math.floor(commandHz / 30))) == 0: if it % 100 == 0: print(it) logger.log([it, rospy.get_rostime().to_sec()] + bc.end_eff_state + bc.truth) it += 1 torques = compute_torque(err, err_dot, cumulativeJointError, Kp, Kd, Ki) messageObj.command = [float(torque) for torque in torques.tolist()] pub.publish(messageObj) rate.sleep()
def _send_pos_command(self, pos): self._try_enable() command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = self._limb.joint_names() #command.position = pos command.command = pos self._cmd_publisher.publish(command)
def move_to_ja(self, waypoints, duration=1.5): """ :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point. Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint :param duration: Total time trajectory will take before ending """ self._try_enable() jointnames = self._limb.joint_names() prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames]) waypoints = np.array([prev_joint] + waypoints) spline = CSpline(waypoints, duration) start_time = rospy.get_time() # in seconds finish_time = start_time + duration # in seconds time = rospy.get_time() while time < finish_time: pos, velocity, acceleration = spline.get(time - start_time) command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.command = pos #command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) #command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self._control_rate.sleep() time = rospy.get_time() for i in xrange(10): command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames #command.position = waypoints[-1] command.command = waypoints[-1] self._cmd_publisher.publish(command) self._control_rate.sleep()
def read_file(file): global joint_command_publisher global g_limb global g_left_gripper global g_endpoint_state f=open(file,"r") lines=f.readlines() RATE=0.1 cmd=JointCommand() cmd.mode=1 g_left_gripper.open() names=["left_s0","left_s1","left_e0","left_e1","left_w0","left_w1","left_w2"] cmd.names=names print cmd joint_str=lines[0].rstrip().split(',') joints=[float(joint) for joint in joint_str] limb_pos=dict(zip(names,joints[0:7])) is_grasping=is_grasp(joints[7]) if(is_grasping): g_left_gripper.close() else: g_left_gripper.open() g_limb.move_to_joint_positions(limb_pos) i=1 while i<len(lines) and not rospy.is_shutdown(): line=lines[i] joint_str=line.rstrip().split(',') joints=[float(joint) for joint in joint_str] if is_grasping != is_grasp(joints[7]): if is_grasp(joints[7]): g_left_gripper.close() is_grasping=True else: g_left_gripper.open() cmd.command=joints[0:7] # print joints[0:7] print i joint_command_publisher.publish(cmd) time.sleep(RATE) i=i+1
def publishIKdata(joint_vals, limb): # available control modes: # POSITION_MODE=1 # VELOCITY_MODE=2 # TORQUE_MODE=3 # RAW_POSITION_MODE=4 msg = JointCommand() msg.names = joint_vals.keys() msg.command = joint_vals.values() msg.mode = JointCommand.POSITION_MODE if limb == 'left': pub_left.publish(msg) if limb == 'right': pub_right.publish(msg)
def left_pub(commands, names): global left_command_pub """ This function sets your speed for the publisher. Use this function to implement your ideas. :params: x:This is the velocity along x-axis in m/sec. Limits: [-0.3,0.3] y:This is the velocity along y-axis in m/sec. Limits: [-0.3,0.3] wz: This is the angular velocity for turning (yaw) in rad/sec. Limits: [-1,1] :return: Returns a speed message """ joint_cmd = JointCommand() joint_cmd.mode = 1 joint_cmd.command = commands joint_cmd.names = names left_command_pub.publish(joint_cmd)
def callback(comm): msg = PoseStamped() msg.header.seq = 0 msg.header.stamp = rospy.get_rostime() msg.header.frame_id = 'base' msg.pose = comm.pose arr = SolvePositionIKRequest() arr.pose_stamp.append(msg) arr.seed_mode = 0 print arr left_ik_addr = '/ExternalTools/left/PositionKinematicsNode/IKService' right_ik_addr = '/ExternalTools/right/PositionKinematicsNode/IKService' if rospy.get_param('/robot/unityros/movelock') == True: return try: left_ik = rospy.ServiceProxy(left_ik_addr, SolvePositionIK) right_ik = rospy.ServiceProxy(right_ik_addr, SolvePositionIK) if comm.limb == 'left': ik = left_ik(arr) else: ik = right_ik(arr) right_limb = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=1) left_limb = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) message = JointCommand() message.mode = 1 message.command = ik.joints[0].position message.names = ik.joints[0].name rospy.loginfo(message) right_limb.publish(message) left_limb.publish(message) except rospy.ServiceException, e: print 'Service call failed: %s' % e
def main(): rospy.init_node('baxter_arm') rospy.loginfo('main method called') rate = rospy.Rate(10) pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) while not rospy.is_shutdown(): msg = JointCommand() msg.mode = msg.POSITION_MODE msg.names = { 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' } msg.command = {0.0, 0.0, 0.1, 0.1, 0.0, 0.0, 0.0} pub.publish(msg) rate.sleep()
def callback(data): time_now = rospy.Time().now() if (time_now.secs - data.header.stamp.secs > 1 | time_now.nsecs - data.header.stamp.nsecs > 500000000): return print "accept command" commandPose = data.pose limb_name = data.header.frame_id limb = baxter_interface.Limb(limb_name) currentPose = limb.endpoint_pose() while not ("position" in currentPose): currentPose = limb.endpoint_pose() newPose = dict() newPose = { 'position': Point( commandPose.position.x, commandPose.position.y, commandPose.position.z, ), 'orientation': Quaternion( commandPose.orientation.x, commandPose.orientation.y, commandPose.orientation.z, commandPose.orientation.w, ), } kinematics = baxter_kinematics(limb_name) inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"]) if not (inverseKinematics == None): inverseKinematicsSolution = list() for num in inverseKinematics.tolist(): inverseKinematicsSolution.append(num) inverseKinematicsSolutionJointCommand = JointCommand() inverseKinematicsSolutionJointCommand.mode = 1 inverseKinematicsSolutionJointCommand.names = limb.joint_names() inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution pub.publish(inverseKinematicsSolutionJointCommand)
def callback(data): #time_now = rospy.Time().now() #if (time_now.secs - data.header.stamp.secs > 1): # rospy.loginfo("time stamp too old, ignore...") # return commandPose = data.pose limb_name = data.header.frame_id; limb = baxter_interface.Limb(limb_name) newPose = dict() newPose = { 'position': Point( commandPose.position.x, commandPose.position.y, commandPose.position.z, ), 'orientation': Quaternion( commandPose.orientation.x, commandPose.orientation.y, commandPose.orientation.z, commandPose.orientation.w, ), } kinematics = baxter_kinematics(limb_name) inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"]) if not (inverseKinematics == None): inverseKinematicsSolution = list() for num in inverseKinematics.tolist(): inverseKinematicsSolution.append(num) inverseKinematicsSolutionJointCommand = JointCommand() inverseKinematicsSolutionJointCommand.mode = 1 inverseKinematicsSolutionJointCommand.names = limb.joint_names() inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution commandSolutionPublisher.publish(inverseKinematicsSolutionJointCommand) # publish to command subscriber to check compute commandCheckPublisher.publish(String("done"))
def callback(data): time_now = rospy.Time().now() if (time_now.secs - data.header.stamp.secs > 1 | time_now.nsecs - data.header.stamp.nsecs > 500000000): return print "accept command" commandPose = data.pose limb_name = data.header.frame_id; limb = baxter_interface.Limb(limb_name) currentPose = limb.endpoint_pose() while not ("position" in currentPose): currentPose = limb.endpoint_pose() newPose = dict() newPose = { 'position': Point( commandPose.position.x, commandPose.position.y, commandPose.position.z, ), 'orientation': Quaternion( commandPose.orientation.x, commandPose.orientation.y, commandPose.orientation.z, commandPose.orientation.w, ), } kinematics = baxter_kinematics(limb_name) inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"]) if not (inverseKinematics == None): inverseKinematicsSolution = list() for num in inverseKinematics.tolist(): inverseKinematicsSolution.append(num) inverseKinematicsSolutionJointCommand = JointCommand() inverseKinematicsSolutionJointCommand.mode = 1 inverseKinematicsSolutionJointCommand.names = limb.joint_names() inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution pub.publish(inverseKinematicsSolutionJointCommand)
def getpose(): pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command', JointCommand) command_msg = JointCommand() command_msg.names = [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] command_msg.mode = JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() joint_positions = rospy.wait_for_message("/robot/joint_states", JointState) qc = (joint_positions.position[9:16]) angles = [qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]] pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command', JointCommand) command_msg = JointCommand() command_msg.names = ['right_e1'] command_msg.command = [0] command_msg.mode = JointCommand.POSITION_MODE control_rate = rospy.Rate(100) start = rospy.get_time() exit = 0 listener2 = tf.TransformListener() #the transformations now = rospy.Time() listener2.waitForTransform("/torso", "/right_hand", now, rospy.Duration(1.0)) (trans08, rot08) = listener2.lookupTransform("/torso", "/right_hand", now) # Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1]) R08 = transformations.quaternion_matrix(rot08) T08 = numpy.vstack((numpy.column_stack( (R08[0:3, 0:3], numpy.transpose(numpy.array(trans08)))), [0, 0, 0, 1])) return (angles, T08)
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(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( "-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:]) #print(sys.argv[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: print(p); 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 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 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) 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) 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 #limb.set_joint_torques(m) for l in range(len(error)): fitness = fitness + error[l] * error[l] i = i + 10 print(fitness) return fitness
def execute_path(data_path): left_limb_commander = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) right_limb_commander = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) head_pan_commander = rospy.Publisher('/robot/head/command_head_pan', HeadPanCommand, queue_size=10) head_nod_commander = rospy.Publisher('/robot/head/command_head_nod', Bool, queue_size=10) rospy.init_node('joint_commander', anonymous=True) traj = get_trajectory(data_path) for joint_state in traj: left_limb_joint_names = [] left_limb_joint_values = [] right_limb_joint_names = [] right_limb_joint_values = [] head_pan_target = None head_nod = False joint_state = literal_eval(joint_state) for joint_name, state in joint_state.items(): if joint_name.startswith('left'): left_limb_joint_names.append(joint_name) left_limb_joint_values.append(state) elif joint_name.startswith('right'): right_limb_joint_names.append(joint_name) right_limb_joint_values.append(state) elif joint_name == "head_pan": head_pan_target = state elif joint_name == "head_nod": if abs(state) > 0: head_nod = True else: head_nod = False left_command = JointCommand() left_command.mode = 1 left_command.names = left_limb_joint_names left_command.command = left_limb_joint_values right_command = JointCommand() right_command.mode = 1 right_command.names = right_limb_joint_names right_command.command = right_limb_joint_values head_pan_command = HeadPanCommand() head_pan_command.target = head_pan_target head_pan_command.speed = 0.1 head_nod_command = Bool() head_nod_command.data = head_nod left_limb_commander.publish(left_command) right_limb_commander.publish(right_command) head_pan_commander.publish(head_pan_command) head_nod_commander.publish(head_nod_command) rospy.loginfo("State reached...")
def main(): rospy.init_node('trayectoria_rombo') #Definimos el RATE de publicacion de mensajes en ROS rate = rospy.Rate(10) #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 = -1.5 limite_inferior_z = -0.8 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]] l = kin.inverse_kinematics(pos, rot) if l != None: msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]] pub.publish(msg) rate.sleep() 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: m = { args.limb + '_w0': theta_i[4], args.limb + '_w1': theta_i[5], args.limb + '_w2': theta_i[6], args.limb + '_e0': theta_i[2], args.limb + '_e1': theta_i[3], args.limb + '_s0': theta_i[0], args.limb + '_s1': theta_i[1] } limb.move_to_joint_positions(m, timeout)
#!/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()
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('kinect_test') vive_data = [] rate = rospy.Rate(30) rospy.Subscriber("unity_locations", String, vive_callback) left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') print('Program Starting! 10 Seconds to move to initial pose') rospy.sleep(6.0) print('Pose Acquired, Demonstrator must hold this position for now!!!!') for i in vive: vive_data.append(float(i)) human_data = np.array(vive_data) print "human_data" print human_data c1 = human_data[0:7] c2 = human_data[7:14] head = human_data[14:21] c1[0] = c1[0] - head[0] c1[1] = c1[1] - head[1] c1[2] = c1[2] - head[2] c2[0] = c2[0] - head[0] c2[1] = c2[1] - head[1] c2[2] = c2[2] - head[2] human_data = np.concatenate((c1, c2)) human_data = np.reshape(human_data, (1,14)) print human_data right_json_file = open('right_nn_without_head.json', 'r') right_loaded_model_json = right_json_file.read() right_json_file.close() right_joint_nn_model = model_from_json(right_loaded_model_json) # load weights into new model right_joint_nn_model.load_weights("right_nn_without_head_wt.h5") right_joint_nn_model.compile(loss = 'mean_squared_error', optimizer='sgd') right_predicted_joints = right_joint_nn_model.predict(human_data)[0] print right_predicted_joints left_neutral_position = {'left_s0':0.948767, 'left_s1':0.901981, 'left_e0':-1.456131, 'left_e1':0.541495, 'left_w0':0.887408, 'left_w1':0.488189, 'left_w2':-2.937190} right_neutral_position = {'right_s0':-1.039272, 'right_s1':0.868233, 'right_e0':1.078005, 'right_e1':0.537660, 'right_w0':-3.045335, 'right_w1':-0.272282, 'right_w2':-1.091811} left.move_to_joint_positions(left_neutral_position) print "left completed" right.move_to_joint_positions(right_neutral_position) right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #print 'Want Robot Ready to Move?[y/n]', #userInput = sys.stdin.readline().strip() #if(userInput == 'y'): #left.move_to_joint_positions(left_neutral_position) #print "left completed" #right.move_to_joint_positions(right_neutral_position) #right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #else: #print("Program exiting...") #return #print('Attempt real time tracking?[y/n]') #if(userInput != 'y'): # print("Program exiting...") # return pub_right = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) pub_left = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) print('Realtime Tracking Starting! Demonstrator can move very slowly now') while not rospy.is_shutdown(): vive_data = [] for i in vive: vive_data.append(float(i)) human_data = np.array(vive_data) c1 = human_data[0:7] c2 = human_data[7:14] head = human_data[14:21] c1[0] = c1[0] - head[0] c1[1] = c1[1] - head[1] c1[2] = c1[2] - head[2] c2[0] = c2[0] - head[0] c2[1] = c2[1] - head[1] c2[2] = c2[2] - head[2] human_data = np.concatenate((c1, c2)) human_data = np.reshape(human_data, (1,14)) print human_data right_predicted_joints = right_joint_nn_model.predict(human_data)[0] #right_end_effector = right_kin.forward_position_kinematics({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #print "end effector:" #print right_end_effector #with open("test_end_effector_vive.csv", 'a') as myfile: # wr = csv.writer(myfile, quoting=csv.QUOTE_ALL) # wr.writerow(np.concatenate((np.array(right_end_effector), human_data[0]))) print "predicted joints:" print right_predicted_joints #right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) command_msg = JointCommand() command_msg.mode = JointCommand.POSITION_MODE command_msg.command = [right_predicted_joints[i] for i in xrange(0, 6)] command_msg.names = ['right_s0', 'right_s1','right_e0','right_e1','right_w0','right_w1', 'right_w2'] pub_right.publish(command_msg) rate.sleep()
while not rospy.is_shutdown(): tr.header.stamp = rospy.Time.now() br.sendTransform(tr) # update pose Md[0, 3] = 0.05 * np.cos(t / 2.) Md[1, 3] = 0.1 * np.sin(t / 2) Md[2, 3] = 0.2 * np.cos(t / 2) Md[:3, :3] = transformations.euler_matrix(.1 * np.cos(t), .1 * np.sin(t / 2), .05 * np.cos(t + 4))[:3, :3] M = np.dot(M0, Md) req.pose_stamp = [matrix_msg(M)] res = ikr(req) cmd.names = res.joints[0].name cmd.command = res.joints[0].position pubr.publish(cmd) req.pose_stamp = [matrix_msg(np.dot(M, Mrl))] res = ikl(req) cmd.names = res.joints[0].name cmd.command = res.joints[0].position publ.publish(cmd) t += dt rospy.sleep(dt)
pos_00 = {'left_w0': -0.17602429520874024, 'left_w1': 0.8264321485290528, 'left_w2': 0.611674838470459, 'left_e0': 0.2320145939025879, 'left_e1': 0.9008302166564942, 'left_s0': -0.09357282795410157, 'left_s1': -0.22511168036499024} pos_10 = {'left_w0': -0.5921165834472657, 'left_w1': 0.8302671004943848, 'left_w2': 0.6864564017944337, 'left_e0': 0.5372767703430176, 'left_e1': 1.03083508828125, 'left_s0': -0.36201946552734376, 'left_s1': -0.20133497817993165} pos_20 = {'left_w0': -1.298898230657959, 'left_w1': 1.1255584018249511, 'left_w2': 0.8356360332458497, 'left_e0': 1.0526943144836427, 'left_e1': 1.0404224681945802, 'left_s0': -0.655009795678711, 'left_s1': 0.03911651004638672} pos_1 = {'left_w0': 1.1662088926574707, 'left_w1': -0.2308641083129883, 'left_w2': -0.09625729432983399, 'left_e0': -1.2026409363281252, 'left_e1': -0.050237870745849615, 'left_s0': 0.3900146148742676, 'left_s1': 0.45559229348144537} pos_2 = {'left_w0': 1.0956457764953613, 'left_w1': -1.4016749433288576, 'left_w2': -0.15569904979248048, 'left_e0': -1.2390729799987794, 'left_e1': -0.04947088035278321, 'left_s0': 0.390781605267334, 'left_s1': 0.12348545328369141} pub_joints = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) rospy.init_node("hand_wave") rate = rospy.Rate(100) cmd_msg = JointCommand() cmd_msg.mode = JointCommand.POSITION_MODE cmd_msg.names = ['left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1'] left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') left.set_joint_position_speed(0.5) print(left.joint_velocities()) print(right.joint_velocities()) #left.set_joint_velocities({'left_w1' : 50}) while not rospy.is_shutdown(): if which_pos: position = dictToList(pos_1) else: position = dictToList(pos_2) cmd_msg.command = position pub_joints.publish(cmd_msg) print(cmd_msg) rospy.sleep(2) which_pos = not which_pos