本文整理汇总了Python中sensor_msgs.msg.JointState.velocity[i]方法的典型用法代码示例。如果您正苦于以下问题：Python JointState.velocity[i]方法的具体用法？Python JointState.velocity[i]怎么用？Python JointState.velocity[i]使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.JointState的用法示例。
在下文中一共展示了JointState.velocity[i]方法的6个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: loop
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = len(self.joints.items())
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.joints:
joint = self.joints[name]
factor = 1
offset = 0
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
msg.position[i] = pos
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
self.pub.publish(msg)
r.sleep()示例2: on_applyWBCRobotPressed
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
def on_applyWBCRobotPressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
#print "Positions: ",joint_state.position
for i,name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for "+name+" = "+str(joint.position)+" at ndx="+str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
#joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)示例3: loop
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
msg = JointState()
msgDisplayRobotState = DisplayRobotState()
msgRobotState = RobotState()
# Publish Joint States
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
msg.position[i] = pos
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
msgRobotState.joint_state = msg
msgDisplayRobotState.state = msgRobotState
self.pub.publish( msgDisplayRobotState )
r.sleep()示例4: loop
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
#.........这里部分代码省略.........
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
if param['name']=='Joint_inter1h' or param['name']=='Joint_inter2h' or param['name']=='Joint_inter3h' or param['name']=='Joint_inter4h':
parent = param['parent']
joint = self.free_joints[parent]
#jointref = self.free_joints[name]
factor = param.get('factor', 1)
offset = -1.82 # -104.27*pi/180 (about 104.27deg to adjust even if 110.47deg into CAO... ?)
#L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(pi/2-joint['position'])
#L = pow(L2,0.5)
#val1 = math.acos( min(1,max( (pow(dist_a,2)+L2-pow(dist_b,2))/(2*L*dist_a) ,-1)) )
#val2 = math.acos( min(1,max( (pow(dist_c,2)+L2-pow(dist_d,2))/(2*L*dist_c) ,-1)) )
#pub.publish("%.2f, %.2f, position_inter1h %.2f"%(val1,val2,joint['position']) )
# function for dependent movment
if has_position and 'position' in joint:
L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
L = pow(L2,0.5)
val1 = math.acos( min(1,max( (pow(dist_a,2)+L2-pow(dist_b,2))/(2*L*dist_a) ,-1)) )
val2 = math.acos( min(1,max( (pow(dist_c,2)+L2-pow(dist_d,2))/(2*L*dist_c) ,-1)) )
if joint['position']>(-1.57):
msg.position[i] = val1 + val2 + offset
else:
msg.position[i] = -val1 + val2 + offset
elif param['name']=='Joint_leg1' or param['name']=='Joint_leg2' or param['name']=='Joint_leg3' or param['name']=='Joint_leg4':
parent = param['parent']
joint = self.free_joints[parent]
#jointref = self.free_joints[name]
factor = param.get('factor', 1)
offset = -1.72 # (-98.55deg=81.45-180)*pi/180 (about 81.45deg instead 87.61deg into CAO... ?)
#L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
#L = pow(L2,0.5)
#val1 = math.acos( min(1,max( (pow(dist_b,2)+L2-pow(dist_a,2))/(2*L*dist_b) ,-1)) )
#val2 = math.acos( min(1,max( (pow(dist_d,2)+L2-pow(dist_c,2))/(2*L*dist_d) ,-1)) )
#pub.publish("%.2f, %.2f, position_inter1 %.2f, L2=%.2f , L+%.2f"%(val1,val2,joint['position'],L2,L) )
# function for dependent movment
if has_position and 'position' in joint:
L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
L = pow(L2,0.5)
val1 = math.acos( min(1,max( (pow(dist_b,2)+L2-pow(dist_a,2))/(2*L*dist_b) ,-1)) )
val2 = math.acos( min(1,max( (pow(dist_d,2)+L2-pow(dist_c,2))/(2*L*dist_d) ,-1)) )
if joint['position']>(-1.57):
msg.position[i] = val1 + val2 + offset
else:
msg.position[i] = -val1 + val2 + offset
else:
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
self.pub.publish(msg)
r.sleep()示例5: loop
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
# create list of joint controllers
joint_controller_dict = {}
for name in self.joint_list:
dxl_topic = '/'+ str( name ) + '_controller/command'
joint_controller_dict[ str( name ) ] = rospy.Publisher(dxl_topic, Float64, queue_size=5)
print( 'Created publisher: ' + dxl_topic )
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
# pos = joint['position'] * factor + offset
msg.position[i] = pos
dx_msg.data = pos
# joint_controller_dict[ str(name) ].publish( dx_msg )
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
self.pub.publish(msg)
r.sleep()示例6: loop
# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import velocity[i] [as 别名]
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for name, joint in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
factor = param.get('factor', 1)
offset = param.get('offset', 0)
# Handle recursive mimic chain
recursive_mimic_chain_joints = [name]
while parent in self.dependent_joints:
if parent in recursive_mimic_chain_joints:
error_message = "Found an infinite recursive mimic chain"
rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent)
sys.exit(-1)
recursive_mimic_chain_joints.append(parent)
param = self.dependent_joints[parent]
parent = param['parent']
offset += factor * param.get('offset', 0)
factor *= param.get('factor', 1)
joint = self.free_joints[parent]
if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
self.pub.publish(msg)
try:
r.sleep()
except rospy.exceptions.ROSTimeMovedBackwardsException:
pass