Limiting the change of arm to a button press [closed]

asked 2018-05-25 07:18:39 -0600

Kashyap.m94 gravatar image

Hey,

I am trying to control my robot arm with an xbox controller and my python code is as follows

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
from moveit_msgs.msg import *
from geometry_msgs.msg import *
from sensor_msgs.msg import *
import time

j1 = 0
a = 0
flag1 = 0
flag2 = 0

def callback(data):
    global j1, a, flag1, flag2
    robot = moveit_commander.RobotCommander()
    group1 = moveit_commander.MoveGroupCommander("igus1")
    end_effector_link =  group1.get_end_effector_link()
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
              moveit_msgs.msg.DisplayTrajectory,queue_size=1)

    print "============ Waiting for RVIZ..."
    print "============ Reference frame: %s" % group1.get_planning_frame()
    print "============ End effector: %s" % group1.get_end_effector_link()
    print "============ Robot Groups: %s" %robot.get_group_names()

    print "============ Printing robot state"
    print robot.get_current_state()
    print "=============="

    group1_variable_values = group1.get_current_joint_values()

    if ((data.buttons[0]==1) and (data.axes[6]==1)):
    if (flag1 == 0):
            a += 1
        flag1 = 1
        j1 = (int(a))*0.1
            print j1
        if (j1 >= 3.1): 
        group1_variable_values[0] = 3.1
        print "Limit reached joint 1"
            else:
        print "changing joint 1"
        group1_variable_values[0] = j1
    plan2 = group1.plan()
        execute2= group1.execute(plan2)
        print "plan complete"

    else:
        flag1 = 0

    if ((data.buttons[0]==1) and (data.axes[6]==-1)):
    if (flag2 == 0):
        a -= 1
        flag2 = 1
        j1 = (int(a))*0.1
            print j1
        if (j1 <= -3.1):
        group1_variable_values[0] == -3.1 
        print "Limit reached joint 1"
            else:
        print "changing joint 1"
        group1_variable_values[0] = j1
    plan2 = group1.plan()
        execute2= group1.execute(plan2)
        print "plan complete"
    else:
        flag2 = 0

    group1.set_joint_value_target(group1_variable_values)

def move_group_python_interface():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)
    rospy.Subscriber("joy", Joy, callback)
    rospy.spin()


if __name__=='__main__':
    try:
           move_group_python_interface()
    except rospy.ROSInterruptException:
        pass

First, I launch my RViz launch file and then I have made a trajectory planner similar to as described in jog_arm file that launches my python code for controlling the robot. The launch file is as follows

<launch>

  <rosparam command="load" file="$(find igus_description)/config/joint_names_igus_1.yaml" />

  <node name="joy_node" pkg="joy" type="joy_node" />

  <node name="igus_server" pkg="igus_trajectory_planning" type="igus_server.py" output="screen" />

</launch>

and the joint_names_igus_1.yaml file is

igus_server:
  simulation: false # Whether the robot is started in simulation environment
  collision_check: true # Check collisions?
  command_in_topic:  igus_server/joy
  command_frame:  base_link  # TF frame that incoming cmds are given in
  incoming_command_timeout:  5  # Stop jogging if X seconds elapse without a new cmd
  joint_topic:  joint_states
  move_group_name:  igus1
  singularity_threshold:  5.5  # Slow down when the condition number hits this (close to singularity)
  hard_stop_singularity_threshold: 12. # Stop when the condition number hits this
  command_out_topic:  igus_controller/igus_joint_speed
  planning_frame:  base_link
  low_pass_filter_coeff:  1.  # Larger --> trust the filtered data more, trust the measurements less.
  publish_period:  0.008  # 1/Nominal publish rate [seconds]
  scale:
    linear:  0.0001  # Max linear velocity. Meters per publish_period. Units is [m/s]
    rotational:  0.0001  # Max angular velocity. Rads per publish_period. Units is [rad/s]
  # Publish boolean warnings to this topic
  warning_topic:  igus_server/warning

and the output displayed on the terminal is below and it keeps printing continuously even if I don't press the buttons on xbox, which causes minute changes in the 'position' values in every cycle.

[ INFO] [1527248279.789739931]: Ready to ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by Kashyap.m94
close date 2018-05-30 12:22:37.955208