Python JointState.effort方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
def main(ra):
    # Create Pose Publisher and Subscriber
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_torque_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) # 1hz
    
    #hardcode home to zero 
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    #joint_msg.name = ['outer_yaw','o', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[]
    joint_msg.velocity = []  
    joint_msg.effort = []
    
  
    #Excitation Trajectory Position Coordinates
    i=0
    q_data1=[-0.2, -0.2, -0.2, -0.2]
    q_data2=[-0.086355,-0.095648,-0.10506,-0.1145,-0.12387,-0.13311,-0.14214,-0.15087,-0.15925,-0.16721,-0.17471,-0.18172,-0.18821,-0.19416,-0.19958,-0.20446,-0.20883,-0.21268,-0.21606,-0.21898,-0.22147,-0.22357,-0.2253,-0.22669,-0.22779,-0.22861,-0.22919,-0.22954,-0.2297,-0.22968,-0.2295,-0.22914,-0.22862,-0.22795,-0.22713,-0.22617,-0.22511,-0.22388,-0.22247,-0.22082,-0.21888,-0.21662,-0.214,-0.21095,-0.20744,-0.20342,-0.19885,-0.19368,-0.18788,-0.1814,-0.17424,-0.16635,-0.15775,-0.14843,-0.13843,-0.12778,-0.11651,-0.10471,-0.092438,-0.079795,-0.066886,-0.053804,-0.040655,-0.027542,-0.014584,-0.0018928,0.010422,0.022271,0.033597,0.044314,0.05438,0.063868,0.072648,0.080705,0.088029,0.09461,0.10049,0.10573,0.11037,0.11443,0.11795,0.12097,0.12353,0.12566,0.12736,0.12866,0.12956,0.13007,0.13023,0.13005,0.12955,0.12874,0.12764,0.12629,0.12472,0.12297,0.12106,0.11903,0.11691,0.11471,0.11247,0.11021,0.10793,0.10564,0.10335,0.10109,0.098863,0.096666,0.094485,0.092317,0.09015,0.087973,0.085786,0.083574,0.081318,0.079023,0.076691,0.074343,0.071992,0.069651,0.067341,0.065096,0.06294,0.060911,0.05905,0.057395,0.055962,0.05478,0.053876,0.053265,0.052956,0.052948,0.053206,0.053699,0.054398,0.055243,0.056172,0.057127,0.058038,0.05881,0.059392,0.059703,0.05959,0.059125,0.058272,0.057011,0.055343,0.053232,0.050668,0.047655,0.044237,0.040451,0.036343,0.031955,0.027341,0.022586,0.017764,0.012952,0.0082075,0.0035806 -0.00086291,-0.0050626,-0.0089787,-0.012584,-0.015859,-0.018781,-0.021356,-0.023586,-0.025488,-0.027074,-0.028361,-0.029398,-0.030214,-0.030849,-0.031337,-0.031706,-0.031991,-0.032234,-0.032468,-0.032704,-0.032954,-0.033233,-0.033555,-0.033912,-0.034301,-0.034697,-0.035081,-0.035422,-0.035697,-0.035867,-0.035877,-0.035676,-0.035207,-0.03442,-0.033253,-0.031662,-0.029576,-0.026944,-0.023725,-0.019887,-0.015407,-0.010266,-0.0044619,0.0020043,0.0091073,0.016811,0.025072,0.033839,0.04304,0.052649,0.062524,0.072569,0.082709,0.092857,0.10293,0.11281,0.12243,0.13173,0.14065,0.14913,0.15713,0.16459,0.1715,0.17786,0.18366,0.1889,0.19362,0.19783,0.20158,0.20492,0.2079,0.21056,0.21295,0.21512,0.21715,0.21904,0.22085,0.22259,0.2243,0.22599,0.22766,0.22932,0.23095,0.23255,0.23409,0.23556,0.23692,0.23812,0.23907,0.2398,0.24025,0.24034,0.24004,0.23926,0.23797,0.23611,0.23359,0.23033,0.22627,0.22136,0.21557,0.20886,0.20118,0.19255,0.18296,0.17243,0.16101,0.14864,0.13538,0.12151,0.10701,0.091903,0.07631,0.060357,0.044198,0.02793,0.011667,-0.0044587,-0.020288,-0.035661,-0.050484,-0.064586,-0.07786,-0.090235,-0.10163,-0.11199,-0.12125,-0.12941,-0.13646,-0.14241,-0.14728,-0.15105,-0.15373,-0.15539,-0.15605,-0.15578,-0.15459,-0.15257,-0.14976,-0.14623,-0.1421,-0.13743,-0.1323,-0.12678,-0.12094,-0.11485,-0.10862,-0.10231,-0.095985,-0.089689,-0.083481,-0.077412,-0.071513,-0.065797,-0.060263,-0.054899,-0.04968,-0.044575,-0.039542,-0.034456,-0.029335,-0.024135,-0.018785,-0.013241,-0.0074516,-0.0014066,0.0049231,0.011561,0.018514,0.025767,0.033291,0.041039,0.048948,0.056956,0.06499,0.072953,0.080753,0.088337,0.095636,0.10252,0.10875,0.11438,0.11943,0.12382,0.12749,0.13041,0.13264,0.1342,0.13509,0.13531,0.13486,0.13379,0.13214,0.12997,0.12732,0.12423,0.12075,0.11694,0.11286,0.10857,0.1041,0.099486,0.09475,0.08992,0.085004,0.080014,0.074935,0.069724,0.064326,0.058826,0.053265,0.047541,0.04157,0.035322,0.028833,0.02214,0.015199,0.0079713,0.00039706,-0.007471,-0.015506,-0.023615,-0.031787,-0.039995,-0.048156,-0.056162,-0.063921,-0.071375,-0.078458,-0.0851,-0.091259,-0.096872,-0.10188,-0.10625,-0.10998,-0.1131,-0.11564,-0.11763,-0.11913,-0.1202,-0.12096,-0.12153,-0.122,-0.12247,-0.12303,-0.12377,-0.12477,-0.12608,-0.1278,-0.1299,-0.13235,-0.13516,-0.13832,-0.14178,-0.14548,-0.14934,-0.15332,-0.15736,-0.16139,-0.16534,-0.16914,-0.17275,-0.17613,-0.1793,-0.18218,-0.18474,-0.18697,-0.18885,-0.19036,-0.19152,-0.19237,-0.19292,-0.19317,-0.19312,-0.19282,-0.19229,-0.19154,-0.19058,-0.18941,-0.18806,-0.18664,-0.18523,-0.18373,-0.18213,-0.18048,-0.17888,-0.17739,-0.17596,-0.17458,-0.17322,-0.17194,-0.17084,-0.16996,-0.16924,-0.16864,-0.16818,-0.16787,-0.16771,-0.16764,-0.16761,-0.1676,-0.16757,-0.1675,-0.16736,-0.16713,-0.16679,-0.1664,-0.16587,-0.16524,-0.16451,-0.1637,-0.16282,-0.16188,-0.16092,-0.15998,-0.15907,-0.15823,-0.15746,-0.15675,-0.1561,-0.15549,-0.15491,-0.15431,-0.15365,-0.15287,-0.15191,-0.15069,-0.14918,-0.14729,-0.14498,-0.14218,-0.13885,-0.13497,-0.13052,-0.12547,-0.11987,-0.11377,-0.10721,-0.10028,-0.093075,-0.085649]
    q_data3=[-0.079852,-0.082139,-0.083106,-0.082646,-0.080594,-0.076762,-0.071134,-0.063814,-0.054815,-0.044135,-0.031832,-0.01809,-0.0031553,0.012866,0.029824,0.047487,0.065601,0.083926,0.10234,0.12065,0.1387,0.15626,0.17319,0.18942,0.20498,0.21978,0.23366,0.24665,0.2588,0.27022,0.28092,0.29103,0.30017,0.30862,0.31667,0.32424,0.33127,0.33778,0.34374,0.34927,0.35453,0.35941,0.36388,0.368,0.37184,0.37538,0.37857,0.38148,0.38422,0.38678,0.38903,0.39112,0.39306,0.39486,0.39652,0.39814,0.39971,0.40112,0.40243,0.40374,0.40498,0.40614,0.40727,0.40825,0.40922,0.41017,0.41094,0.41154,0.41218,0.41281,0.41336,0.4141,0.41439,0.4145,0.41497,0.41573,0.41643,0.41699,0.41767,0.41851,0.41953,0.42069,0.42196,0.42346,0.42504,0.42669,0.42843,0.43035,0.43243,0.43441,0.43623,0.43786,0.43943,0.44093,0.44214,0.44285,0.44304,0.44289,0.4424,0.44132,0.43955,0.43711,0.43405,0.4307,0.42677,0.4221,0.41688,0.41125,0.40524,0.39895,0.39239,0.38549,0.37853,0.37166,0.3646,0.35757,0.35082,0.34449,0.33832,0.33227,0.32652,0.32142,0.31679,0.31253,0.30862,0.30518,0.30219,0.29973,0.29775,0.2961,0.29478,0.29393,0.29349,0.29333,0.29348,0.29395,0.29472,0.29585,0.29723,0.29875,0.30057,0.30268,0.30472,0.30738,0.31034,0.31319,0.31598,0.31898,0.32227,0.32572,0.32922,0.33272,0.33621,0.33977,0.34334,0.34694,0.35048,0.35394,0.35726,0.36057,0.36358,0.36645,0.36937,0.37217,0.37462,0.37681,0.3788,0.38067,0.3824,0.38385,0.38505,0.386,0.38683,0.3876,0.3882,0.38855,0.38878,0.38903,0.38924,0.3894,0.38954,0.38964,0.38973,0.38987,0.38996,0.39024,0.39045,0.39056,0.39054,0.39057,0.39061,0.39054,0.39022,0.38975,0.38923,0.38866,0.38787,0.3868,0.38548,0.38407,0.38257,0.38089,0.37898,0.37685,0.37461,0.37221,0.36953,0.3665,0.36309,0.3591,0.35474,0.3498,0.34393,0.33692,0.32875,0.31955,0.30912,0.29724,0.28371,0.26859,0.25196,0.23391,0.21438,0.19339,0.17107,0.14772,0.12357,0.098775,0.073515,0.04809,0.022717,-0.0019257,-0.025823,-0.04901,-0.071158,-0.091765,-0.11066,-0.12781,-0.14327,-0.157,-0.16883,-0.17863,-0.18654,-0.19287,-0.19781,-0.20126,-0.20332,-0.20431,-0.2044,-0.20379,-0.20265,-0.20106,-0.19905,-0.19676,-0.19444,-0.19206,-0.18958,-0.18694,-0.18415,-0.18108,-0.17764,-0.17373,-0.16921,-0.16387,-0.15758,-0.15023,-0.1417,-0.13161,-0.11999,-0.10746,-0.093137,-0.076579,-0.05832,-0.038796,-0.018042,0.0040629,0.027463,0.051969,0.077223,0.10292,0.12895,0.15479,0.18029,0.20545,0.23013,0.25388,0.27614,0.29682,0.31604,0.33383,0.34993,0.36403,0.37608,0.38615,0.39409,0.40046,0.40509,0.4075,0.40754,0.40552,0.40177,0.39601,0.38801,0.37773,0.36534,0.35088,0.33431,0.31538,0.29427,0.27131,0.24652,0.2198,0.19133,0.16153,0.13065,0.098926,0.066691,0.034325,0.0020559,-0.029857,-0.060853,-0.090528,-0.11874,-0.14525,-0.16976,-0.19203,-0.21181,-0.22901,-0.24362,-0.25566,-0.26511,-0.27201,-0.27647,-0.2787,-0.27896,-0.27737,-0.27409,-0.26965,-0.26386,-0.25687,-0.24871,-0.2406,-0.23276,-0.22456,-0.21582,-0.20693,-0.19843,-0.19045,-0.18292,-0.17577,-0.16904,-0.16281,-0.15715,-0.15211,-0.14776,-0.14411,-0.14096,-0.13819,-0.13587,-0.13402,-0.13255,-0.13125,-0.12989,-0.12842,-0.12654,-0.1239,-0.12097,-0.11708,-0.11161,-0.10524,-0.098217,-0.089336,-0.077418,-0.063179,-0.04798,-0.031811,-0.01369,0.0068895,0.029129,0.052422,0.076567,0.1018,0.12787,0.15402,0.17991,0.20572,0.23111,0.25527,0.27779,0.29889,0.3186,0.33655,0.35244,0.36616,0.37776,0.3874,0.39513,0.40098,0.40494,0.40711,0.40757,0.40651,0.40409,0.40054,0.39578,0.38979,0.38254,0.37412,0.36496,0.35435,0.34238,0.32931,0.3152,0.29976,0.28285,0.26448,0.24487,0.22398,0.20186,0.17857,0.15437,0.12942,0.10371,0.076993,0.050524,0.024549,-0.0014408,-0.027645,-0.05315,-0.077226,-0.10005,-0.12186,-0.14242,-0.16106,-0.17779,-0.19311,-0.20692,-0.21881,-0.22894,-0.23753,-0.24408,-0.24874,-0.25271,-0.25664,-0.2598,-0.26109,-0.26095,-0.26058,-0.26071,-0.26048,-0.25967,-0.25854,-0.2577,-0.25711,-0.25636,-0.2554,-0.25472,-0.25436,-0.25387,-0.25307,-0.25219,-0.2515,-0.25087,-0.25005,-0.24893,-0.24764,-0.24649,-0.24529,-0.24406,-0.24276,-0.24139,-0.23999,-0.2387,-0.23762,-0.23689,-0.23632,-0.23591,-0.23565,-0.23566,-0.23601,-0.23656,-0.23719,-0.23776,-0.2382,-0.23847,-0.23848,-0.23807,-0.23715,-0.2354,-0.2328,-0.22933,-0.225,-0.21972,-0.21329,-0.20536,-0.19699,-0.18807,-0.17793,-0.16645,-0.15449,-0.14255]

    q_output=[None]*len(q_data1)
    qd_output=[None]*len(q_data1)
    torque_output=[None]*len(q_data1)
    while not rospy.is_shutdown():
      if i>len(q_data1)-1:
        i=0;
      joint_msg.position = []
      joint_msg.velocity = []  
      joint_msg.effort=[q_data1[i],0,0,0,0,0,0]
      #joint_msg.effort = [joint_sub.effort[0],joint_sub.effort[1],joint_sub.effort[2],joint_sub.effort[3],joint_sub.effort[4],joint_sub.effort[5],joint_sub.effort[6],joint_sub.effort[7]]
      print(joint_msg)
      joint_pub.publish(joint_msg)

      q_output[i]=joint_sub.position
      qd_output[i]=joint_sub.velocity
      torque_output[i]=joint_sub.effort

      rospy.sleep(1/float(ra))
      i+=1
    rate.sleep()
    
    #Print Code
    with open('data_position.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(q_output[i])
    with open('data_velocity.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(qd_output[i])
    with open('data_torque.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(torque_output[i])    

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
    def execute(self, userdata):
        # rospy.loginfo('Executing state %s', self.__class__.__name__)
        # self.active = True
        # self.first_call = True

        # # wait for some time to read from the topic
        # # TODO: replace with a service call
        # rospy.sleep(3)
        # userdata.obj_list = self.obj_list

        # self.active = False
        # return 'succeeded'
        rospy.loginfo("Executing state %s", self.__class__.__name__)

        self.obj_list = []

        i = 1
        for view in userdata.view_list:

            rospy.loginfo("%i view: set PTU to %s", i, view)

            joint_state = JointState()
            joint_state.header.frame_id = "tessdaf"
            joint_state.name = ["pan", "tilt"]
            joint_state.position = [float(view[0]), float(view[1])]
            joint_state.velocity = [float(1.0), float(1.0)]
            joint_state.effort = [float(1.0), float(1.0)]

            self.ptu_cmd.publish(joint_state)

            # wait until PTU has finished or point cloud get screwed up
            rospy.sleep(2)

            rospy.loginfo("%i view: receive from semantic camera", i)
            self.active = True
            self.first_call = True

            # wait for some time to read once from the topic and store it onto self.pointcloud
            # TODO: replace with a service call
            rospy.sleep(2)

            self.active = False

            i = i + 1

        userdata.obj_list = self.obj_list

        # back to original position
        joint_state = JointState()
        joint_state.header.frame_id = "tessdaf"
        joint_state.name = ["pan", "tilt"]
        joint_state.position = [float(0.0), float(0.0)]
        joint_state.velocity = [float(1.0), float(1.0)]
        joint_state.effort = [float(1.0), float(1.0)]

        self.ptu_cmd.publish(joint_state)

        return "succeeded"

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
def sim_robot():

    rospy.init_node("simulated_robot")
    pub = rospy.Publisher("joint_states", JointState, queue_size=5)

    rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
    rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
    rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
    rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
    rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
    rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))

    msg = JointState()
    msg.header = Header(stamp=rospy.Time.now())
    msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
    msg.position = [0] * 6
    msg.velocity = [0, 0, 0, 0, 0, 0]
    msg.effort = [0, 0, 0, 0, 0, 0]

    def joint_cb(msg_in, id):
        msg.header = Header(stamp=rospy.Time.now())
        msg.position[id] = msg_in.data

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def update(self):
     if not self.driver_status == 'DISCONNECTED':
         # Get Joint Positions
         self.current_joint_positions = self.rob.getj()
         msg = JointState()
         msg.header.stamp = rospy.get_rostime()
         msg.header.frame_id = "robot_secondary_interface_data"
         msg.name = self.JOINT_NAMES
         msg.position = self.current_joint_positions
         msg.velocity = [0]*6
         msg.effort = [0]*6
         self.joint_state_publisher.publish(msg)
         
         # Get TCP Position
         tcp_angle_axis = self.rob.getl()
         # Create Frame from XYZ and Angle Axis
         T = PyKDL.Frame()   
         axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
         # Get norm and normalized axis
         angle = axis.Normalize()
         # Make frame
         T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
         T.M = PyKDL.Rotation.Rot(axis,angle)
         # Create Pose
         self.current_tcp_pose = tf_c.toMsg(T)
         self.current_tcp_frame = T
         self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def set_stiffness(self):
     stiffness_message = JointState()
     stiffness_message.name = ["HeadYaw", "HeadPitch"]
     stiffness_message.position = [0,0]
     stiffness_message.velocity = [0,0]
     stiffness_message.effort   = [1,1]
     self.stiffness_publisher.publish(stiffness_message)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def generate_views(self):        
     rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
     try:
         resp = self.nav_goals(10, self.inflation_radius, self.roi)
         
         if not resp.goals.poses:
             self.views = []
             return
         for j in range(len(resp.goals.poses)):
             for i in range(0,self.views_at_pose):
                 pose = resp.goals.poses[j]
                 pan = random.uniform(self.min_pan, self.max_pan)
                 tilt = random.uniform(self.min_tilt, self.max_tilt)
                 jointstate = JointState()
                 jointstate.name = ['pan', 'tilt']
                 jointstate.position = [pan, tilt]
                 jointstate.velocity = [self.velocity, self.velocity]
                 jointstate.effort = [float(1.0), float(1.0)]
                 resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
                 p = Pose()
                 view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
                 self.views.append(view)
             
     except rospy.ServiceException, e:
         rospy.logerr("Service call failed: %s" % e)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def __on_packet(self, buf):
     global last_joint_states, last_joint_states_lock
     now = rospy.get_rostime()
     #stateRT = RobotStateRT.unpack(buf)
     stateRT = RobotStateRT_V32.unpack(buf)
     self.last_stateRT = stateRT
     
     msg = JointState()
     msg.header.stamp = now
     msg.header.frame_id = "From real-time state data"
     msg.name = joint_names
     msg.position = [0.0] * 6
     for i, q in enumerate(stateRT.q_actual):
         msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
     msg.velocity = stateRT.qd_actual
     msg.effort = [0]*6
     pub_joint_states.publish(msg)
     with last_joint_states_lock:
         last_joint_states = msg
     
     wrench_msg = WrenchStamped()
     wrench_msg.header.stamp = now
     wrench_msg.wrench.force.x = stateRT.tcp_force[0]
     wrench_msg.wrench.force.y = stateRT.tcp_force[1]
     wrench_msg.wrench.force.z = stateRT.tcp_force[2]
     wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
     wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
     wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
     pub_wrench.publish(wrench_msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
    def command_joint_torques(self, desired_torques):
        """
        Command a desired torque for each joint.

        The desired torque must be the correct dimensionality
        (self._num_joints). Similarly to poses, we do not sanity-check
        the inputs. As a rule of thumb, values between +- 0.5 are fine.

        :param desired_torques: The desired joint torques.
        :return: True if message is published, False otherwise.
        """

        # Check that the desired torque vector can have len() applied to it,
        # and that the number of dimensions is the same as the number of
        # joints. This prevents passing singletons or incorrectly-shaped lists
        # to the message creation (which does no checking).
        if (not hasattr(desired_torques, '__len__') or
                len(desired_torques) != self._num_joints):
            rospy.logwarn('Desired torques must be a {}-d array: got {}.'
                          .format(self._num_joints, desired_torques))
            return False

        msg = JointState()  # Create and publish
        try:
            msg.effort = desired_torques
            self.pub_joint.publish(msg)
            rospy.logdebug('Published desired torques.')
            return True
        except rospy.exceptions.ROSSerializationException:
            rospy.logwarn('Incorrect type for desired torques: {}.'.format(
                desired_torques))
            return False

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def publishJointState(self):
     msg = JointState()
     msg.header.stamp = rospy.Time.now()
     msg.name = self.jointNames
     msg.position = self.getJointAngles()
     msg.effort = self.getLoads()
     self.jointStatePub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def loop(self):
     hz = 20
     r = rospy.Rate(hz)
     msg = JointState()
     for i in range(8):
         msg.name.append("q"+str(i+1))
         
     while not rospy.is_shutdown() and not self.endthread:
         br = tf.TransformBroadcaster()
         msg.header.stamp=rospy.Time.now()
         msg.position = 8 * [0.0]
         msg.effort = 8 * [0.0]
         self.torque[6]=0.1
         for i in range(8):
             msg.position[i]=self.q_desired[i]
             msg.effort[i]=self.torque[i]
         if(self.publishInfo):
             self.publisher.publish(msg)     
             
         if(self.publishDesiredInfo):
             br.sendTransform((self.wTp_desired[0],
                              self.wTp_desired[1],
                              self.wTp_desired[2]),
                              tf.transformations.quaternion_from_euler(self.wTp_desired[3],
                                                                       self.wTp_desired[4],
                                                                       self.wTp_desired[5]),
                              rospy.Time.now(),
                              self.child_frame,
                              self.parent_frame)
             # Add in the tf transform now
         
         self.master.update_idletasks()    
         r.sleep()            

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
    def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
        if len(jointsToSweep) != 0:    
            for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
                currPositions[joint_list.index(jointsToSweep[0])] 
                currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
                if (len(jointsToSweep) > 1):
                    self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)

                # Combine message and publish
                msg = JointState()
                msg.header.stamp = rospy.Time.now()
                msg.name = self.joint_list
                # print self.free_joints
                msg.position = currPositions
                msg.velocity = [0.0]*len(maxs)
                msg.effort = [0.0]*len(maxs)
                time.sleep(0.0001)
                self.pub.publish(msg)
                # Get End Effector Positions
                rate = rospy.Rate(20.0)
                try:
                    (trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
                    if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
                        self.csvWriter.writerow(trans)
                    self.currX = trans[0]
                    self.currY = trans[1]
                    self.currZ = trans[2]
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                    print "ERROR: looking up tf"
                    time.sleep(0.5) # Sleep for a bit to let system catch up
                    continue

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
def generateJointStatesFromRegisterStateAndPublish():
    rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState)

    global currentJointState 
    currentJointState = JointState()
    currentJointState.header.frame_id = "robotiq_s_model from real-time state data"
    currentJointState.name = ["finger_1_joint_1", 
                                "finger_1_joint_2", 
                                "finger_1_joint_3", 
                                "finger_2_joint_1", 
                                "finger_2_joint_2", 
                                "finger_2_joint_3", 
                                "finger_middle_joint_1", 
                                "finger_middle_joint_2", 
                                "finger_middle_joint_3", 
                                "palm_finger_1_joint", 
                                "palm_finger_2_joint"]

    currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering?
    rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False)
    rate = rospy.Rate(100) # publishes at 100Hz

    while not rospy.is_shutdown():
        currentJointState.header.stamp = rospy.Time.now()
        pub.publish(currentJointState)
        rate.sleep()

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def publishJointStates(self):
     jState = JointState()
     jState.header.stamp = rospy.Time.now()
     jState.name = self.names
     jState.position = self.positions
     jState.velocity = self.velocities
     jState.effort = self.efforts
     self.pub.publish(jState)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
def get_new_JointState(pan, tilt):
	j = JointState()
	j.header.stamp = rospy.Time.now()
	j.name = ["panservo", "tiltservo"]
	j.position = [pan, tilt]
	j.velocity = []
	j.effort = []
	return j

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import effort [as 别名]
 def publishJointStates(self):
     # rospy.loginfo(self.jointPositions)
     jState = JointState()
     jState.header.stamp = rospy.Time.now()
     jState.name = self.jointNamesFromConfig
     jState.position = self.jointPositions
     jState.velocity = self.jointVelocities
     jState.effort = self.jointAccelerations
     self.joint_state_pub.publish(jState)