No change in joint positions
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 ...