Example #1
0
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)
Example #3
0
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")