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 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")