本文整理汇总了Python中dynamixel_msgs.msg.JointState.goal_pos方法的典型用法代码示例。如果您正苦于以下问题：Python JointState.goal_pos方法的具体用法？Python JointState.goal_pos怎么用？Python JointState.goal_pos使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类dynamixel_msgs.msg.JointState的用法示例。
在下文中一共展示了JointState.goal_pos方法的8个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _BroadcastJointStateinfo_P1
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = ((321-float(lineParts[1]))*0.008)-0.15
P2 = self.left_tilt
P3 = float(lineParts[3])# current
P4 = 0#float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P1_MotorPublisher.publish(Motor_State)
self._left_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))示例2: _BroadcastJointStateinfo_P1
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position
P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_pan_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))示例3: _BroadcastJointStateinfo_P4
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P4(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57)
P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = float(lineParts[3])
P4 = float(lineParts[4])
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P4_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
#rospy.logwarn(val)
Joint_State = JointState()
Joint_State.name = "right_arm_rotate_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P4_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))示例4: msg_callback_left_finger
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def msg_callback_left_finger(self, msg):
out_msg = JointState()
out_msg.header = msg.header
out_msg.goal_pos = msg.set_point
out_msg.current_pos = msg.process_value
out_msg.error = msg.error
#we still need to obtain the speed information about the joint...
#motor IDs are filled according to a joint of the arm
out_msg.motor_ids.append(8)
#other stuff that is not known
out_msg.load=0
out_msg.is_moving=0 # we should obtain this information somehow
out_msg.motor_temps.append(0)
self.pub_left_finger.publish(out_msg)示例5: _HandleJoint_1_Command
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck =(data)
self.gear_ratio = 0.3
new_pos =float (data.current_pos) * self.gear_ratio
new_goal=float (data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name =("head_tilt_mod_joint")
jointstate.motor_ids =(neck.motor_ids)
jointstate.motor_temps =(neck.motor_temps)
jointstate.goal_pos =(new_goal)
jointstate.current_pos =(new_pos)
jointstate.velocity =(neck.velocity)
jointstate.load =(neck.load)
jointstate.is_moving =(neck.is_moving)
jointstate.error =(new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)示例6: _BroadcastJointStateinfo_P2
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if partsCount < 7:
pass
try:
off = 299
A1 = float(lineParts[1]) - off
P1 = 0 - ((A1 * 0.00174532925) - 0)
A2 = float(lineParts[2]) - off
P2 = 0 - ((A2 * 0.00174532925) - 0)
P3 = float(lineParts[3]) # current
P4 = 0 # float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
self._right_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now() # .stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
# rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))示例7: _BroadcastJointStateinfo_P2
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
cal = 0
A1 = (1023 + cal) - float(lineParts[1])
P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position
A2 = (1023 + cal) - float(lineParts[1])
P2 = (A2* 0.003070961)#float(lineParts[2])# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))示例8: _BroadcastJointStateinfo_P3
# 需要导入模块: from dynamixel_msgs.msg import JointState [as 别名]
# 或者: from dynamixel_msgs.msg.JointState import goal_pos [as 别名]
def _BroadcastJointStateinfo_P3(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
#offset = Float(-1.57)
P1 = (float(lineParts[1])/1000)-0.6
P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = 0#float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P3_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._left_lift_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P3_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))