No change in joint positions

edit

Hey guys,

I am trying to control my robot arm through xbox 360 controller and for the same I have visualized the arm in rviz and I have a small trial code in python for controlling the motion of joints. It's a 5 DOF arm.

After connecting the controller, I first launch the demo.launch file and then rosrun joy joy_node followed by the python code for controlling the motion of the joints. When I run the python code, the joint values are all 0 at the beginning. But, as I press the controller buttons, there is no change in the arm's pose.

Could anyone guide me as to where am I doing the mistake? I also would like to know how can I visualize the joint movements, that I give through my controller, in rviz?

The python code is:

#!/usr/bin/env python

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

    def callback(data):

        robot = moveit_commander.RobotCommander()

        #scene = moveit_commander.PlanningSceneInterface()

        group1 = moveit_commander.MoveGroupCommander("igus_planning")
        end_effector_link =  group1.get_end_effector_link()

        display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                  moveit_msgs.msg.DisplayTrajectory,queue_size=50)



        print "============ Waiting for RVIZ..."
        #rospy.sleep(1)
        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 "=============="
        #print end_effector_link

        print "============ current pose..."
        print group1.get_current_pose()


        group1_variable_values = group1.get_current_joint_values()

        ###################   0 and 6   ########################
        while data.buttons[0]==1:
            if (data.axes[6]==1):
                if (group1_variable_values[0] == 3.1): 
                    print "Limit reached joint 1"
                else:
                    print "changing joint 1"
                group1_variable_values[0] += 0.1
            if (data.axes[6]==-1):
                if (group1_variable_values[0] == -3.1): 
                    print "Limit reached joint 1"
                else:
                    print "changing joint 1"
                group1_variable_values[0] -= 0.1

        group1.set_joint_value_target(group1_variable_values)

        plan2 = group1.plan()
        print group1.get_planning_time()
        print "plan complete"

    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

The output that I am getting even after pressing the controller buttons are:

[ INFO] [1525969960.203313336]: Ready to take commands for planning group igus1.
============ Waiting for RVIZ...
============ Reference frame: /world
============ End effector: ee_link
============ Robot Groups: ['igus1']
============ Printing robot state
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  name: [j1, j2, j3, j4, ee_joint]
  position: [0.0, 0.0, 0.0, 0.0, 0.0]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False
==============
5.0
plan complete
[ INFO] [1525969961.417759417]: Ready to take commands for planning group igus1.
============ Waiting for RVIZ...
============ Reference frame: /world
============ End effector: ee_link
============ Robot Groups: ['igus1']
============ Printing robot state
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  name: [j1, j2, j3, j4, ee_joint]
  position: [0.0, 0.0, 0.0, 0.0, 0.0]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False
==============
5.0
plan complete

and in ... (more)