def main():
rospy.init_node("cmd_arm_vel_imp", anonymous=True)
pub = rospy.Publisher(options.topic,
MultiJointVelocityImpedanceCommand,
queue_size=1,
latch=True)
cmd = MultiJointVelocityImpedanceCommand()
cmd.header.stamp = rospy.Time.now()
cmd.velocity = [0.0] * 7
cmd.velocity[int(options.number)] = float(options.velocity)
cmd.stiffness = [float(options.stiffness)] * 7
cmd.damping = [float(options.damping)] * 7
rospy.loginfo("Publishing to:")
print options.topic
rospy.loginfo("Publishing the following command:")
print cmd
while (not rospy.is_shutdown()):
pub.publish(cmd)
rospy.sleep(0.05)
rospy.loginfo("Done publishing")
def command_vel_step(pub, amplitude, joint_nr, duration):
cmd = MultiJointVelocityImpedanceCommand()
cmd.velocity = [ 0.0 ] * 7
cmd.velocity[ joint_nr ] = amplitude
cmd.stiffness = [ float(options.stiffness) ] * 7
cmd.damping = [ float(options.damping) ] * 7
rospy.loginfo("Publishing the following command:")
print cmd
start_time = rospy.Time.now()
while((not rospy.is_shutdown()) and (rospy.Time.now() < start_time + duration)):
cmd.header.stamp = rospy.Time.now()
pub.publish(cmd)
rospy.sleep(0.05)
rospy.loginfo("Done publishing")
def main():
rospy.init_node("cmd_arm_vel_imp", anonymous=True)
pub = rospy.Publisher(options.topic, MultiJointVelocityImpedanceCommand, queue_size=1, latch=True)
cmd = MultiJointVelocityImpedanceCommand()
cmd.header.stamp = rospy.Time.now()
cmd.velocity = [ float(options.velocity) ] * 7
cmd.stiffness = [ float(options.stiffness) ] * 7
cmd.damping = [ float(options.damping) ] * 7
rospy.loginfo("Publishing to:")
print options.topic
rospy.loginfo("Publishing the following command:")
print cmd
while(not rospy.is_shutdown()):
pub.publish(cmd)
rospy.sleep(0.05)
