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 __init__(self): self.activation_thresh = rospy.get_param('~activation_threshold', 0.05) # rad self.command_thresh = rospy.get_param('~command_threshold', 0.3) # rad/s self.stiffness = rospy.get_param('~stiffness', 80.0) # Nm/rad self.p_gain = rospy.get_param('~p_gain', 10.0) # 1 feedback_str = "activation threshold: %s" % self.activation_thresh rospy.loginfo(feedback_str) feedback_str = "command threshold: %s" % self.command_thresh rospy.loginfo(feedback_str) feedback_str = "stiffness: %s" % self.stiffness rospy.loginfo(feedback_str) feedback_str = "p-gain: %s" % self.p_gain rospy.loginfo(feedback_str) self.cmd = MultiJointVelocityImpedanceCommand() self.cmd.velocity = [0.0] * 7 self.cmd.stiffness = [self.stiffness] * 7 self.cmd.damping = [0.7] * 7 self.pub = rospy.Publisher('~velocity_command', MultiJointVelocityImpedanceCommand, queue_size=1, latch=False, tcp_nodelay=True) self.sub = rospy.Subscriber('~beasty_state', rcu2tcu, self.state_callback, queue_size=1, tcp_nodelay=True)
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)
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")