Moving Manipulator in Gazebo or Rviz

edit

Hi, I'm relatively new to ROS, and I need to simulate a robotic arm to track velocities and avoid obstacles. I've written all the mathy algorithms down to find the inverse kinematics that make the arm do what I want, but I'm having a lot of trouble with getting the arm to move in a simulator. My urdf arm model spawns correctly in an empty world environment in gazebo, but I get "service transport errors" whenever I try to use the service gazebo/get_link_state or gazebo/set_link_state using rospy. My question is two fold:

1.) In order to get the arm to move in the simulator, should I be using gazebo/set_link_state and set either the pose or the twist, or should I be using gazebo/apply_effort to apply the torque to the joints.

2.) Is there an easier way to move a robotic arm in a simulator? Should I be using Rviz instead of gazebo?

I know there's lots of prebuilt stacks for manipulator navigation with the pr2 and a number of other robots, but I need to write my own control program for this project. If you have experience with this, please help.

Here's a traceback of me trying to move the manipulator with rospy ServiceProxy calls to gazebo/set_link_state:

Traceback (most recent call last):
  File "test_move.py", line 11, in <module>
    MC.gazebo_set_links()
  File "/home/josh/Documents/iRobot_Ex/manipulator_controller.py", line 122, in gazebo_set_links
    self.set_link_state(link_state)
  File "/opt/ros/fuerte/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 432, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/fuerte/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 522, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details

I set up the ServiceProxy and stuffed the link_state like this:

rospy.wait_for_service('gazebo/set_link_state')
        try:
            self.set_link_state = rospy.ServiceProxy('gazebo/set_link_state',SetLinkState)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

    def gazebo_set_links(self):
        #This assumes that the DH origins are the same as the gazebo origins
        for i in xrange(self.n):
            link_state = LinkState()
            link_state.link_name = 'link'+str(i)
            link_state.pose.position.x = self.H[i][0,3]
            link_state.pose.position.y = self.H[i][1,3]
            link_state.pose.position.z = self.H[i][2,3]
            link_quat = rot_to_quat(self.H[i][0:3,0:3])
            link_state.pose.orientation.w = link_quat[0]
            link_state.pose.orientation.x = link_quat[1]
            link_state.pose.orientation.y = link_quat[2]
            link_state.pose.orientation.z = link_quat[3]
            #default values for twist is 0?
            link_state.twist.linear.x = 0
            link_state.twist.linear.y = 0
            link_state.twist.linear.z = 0
            link_state.twist.angular.x = 0
            link_state.twist.angular.y = 0
            link_state.twist.angular.z = 0
            link_state.reference_frame = 'base'
            self.set_link_state(link_state)