$search

controller_interface.py

Go to the documentation of this file.

import roslib; roslib.load_manifest("pr2_dremel_server")

import pr2_controllers_msgs.msg
from pr2_controller_manager import pr2_controller_manager_interface
from std_msgs.msg import *


import pdb

import actionlib
import sensor_msgs.msg
import kinematics_msgs.msg
import geometry_msgs
import trajectory_msgs
import rospy

arm_seeds = (-0.135, -0.183, -3.202, -1.598, 3.161, -1.318, -3.053)
arm_seeds_without_shoulder = (-0.135, -3.202, -1.598, 3.161, -1.318, -3.053)

r_arm_joints =  ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']
r_arm_joints_without_shoulder =  ['r_shoulder_pan_joint', 'r_elbow_flex_joint', 'r_upper_arm_roll_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']

class JointForceController:
  def __init__(self, joint_name):
    self.controller_name = joint_name + "_joint_force_controller"
      
    rospy.set_param(self.controller_name + '/type', 'pr2_plotter/JointForceController')
    rospy.set_param(self.controller_name + '/joint', joint_name)
    rospy.set_param(self.controller_name + '/max_velocity', 0.2)
    
    rospy.set_param(self.controller_name + '/position/pid', rospy.get_param("r_arm_controller/gains/r_shoulder_lift_joint") )
    rospy.set_param(self.controller_name + '/velocity/pid', rospy.get_param("r_arm_controller/gains/r_shoulder_lift_joint") )
    rospy.set_param(self.controller_name + '/velocity/pid/p', 5)
    rospy.set_param(self.controller_name + '/velocity/pid/i', 20)
    rospy.set_param(self.controller_name + '/velocity/pid/d', 0)
    rospy.set_param(self.controller_name + '/velocity/pid/i_clamp', 60)

    pr2_controller_manager_interface.load_controller(self.controller_name)
    
    self.force_pub = rospy.Publisher("%s/force" % self.controller_name, Float64)
    self.position_pub = rospy.Publisher("%s/position" % self.controller_name, Float64)
    self.running = False
        
  def __del__(self):
    self.shutdown()

  def shutdown(self):
    self.stop()
    self.load()
    pr2_controller_manager_interface.unload_controller(self.controller_name)

  def stop(self):
    pr2_controller_manager_interface.stop_controller(self.controller_name)
    self.running = False

  def load(self):
    pr2_controller_manager_interface.load_controller(self.controller_name)

  def unload(self):
    pr2_controller_manager_interface.unload_controller(self.controller_name)

  def start(self):
    pr2_controller_manager_interface.start_controller(self.controller_name)
    self.running = True

  def set_position_pid(self, p, i, d):
    rospy.set_param(self.controller_name + '/position/pid/p', p )
    rospy.set_param(self.controller_name + '/position/pid/i', i ) 
    rospy.set_param(self.controller_name + '/position/pid/d', d ) 

  def set_velocity_pid(self, p, i, d):
    rospy.set_param(self.controller_name + '/velocity/pid/p', p )
    rospy.set_param(self.controller_name + '/velocity/pid/i', i ) 
    rospy.set_param(self.controller_name + '/velocity/pid/d', d ) 
        
  def force(self, force):
    if not self.running:
      self.start()
    self.force_pub.publish(Float64(force))
    
  def position(self, position):
    if not self.running:
      self.start()
    self.position_pub.publish(Float64(position))

class ArmPositionController:
  def __init__(self, controller_name):
    self.controller_name = controller_name
    self.running = False

  def stop(self):
    pr2_controller_manager_interface.stop_controller(self.controller_name)
    self.running = False

  def start(self):
    pr2_controller_manager_interface.start_controller(self.controller_name)
    self.running = True

  def load(self):
    pr2_controller_manager_interface.load_controller(self.controller_name)

class Arm:
    def __init__(self, controller, ikPath):
        self.controller = controller
        self.trajClient = actionlib.SimpleActionClient(controller+"/joint_trajectory_action", pr2_controllers_msgs.msg.JointTrajectoryAction)

        self.joint_names = self.getJointNames()
        self.cAngles = self.getJointAngles()
        
        self.ikPath = ikPath
        self._runIK = rospy.ServiceProxy(ikPath + "/get_ik", kinematics_msgs.srv.GetPositionIK)
        
       # self._getIKInfo = rospy.ServiceProxy(ikPath+ "/get_ik_solver_info", kinematics_msgs.msg.KinematicSolverInfo)
       
    def sendTraj(self, traj, wait = True):
        """ Send the joint trajectory goal to the action server"""
        if wait:
          self.trajClient.send_goal_and_wait(traj)
        else:
          self.trajClient.send_goal(traj)
    
    def getState(self):
        """Returns the current state of action client
        """
        return self.trajClient.get_state()    

    def getJointNames(self):
        """ Contacts param server to get controller joints """
        return rospy.get_param(self.controller + "/joints");

    def getJointAngles(self):
        msg = rospy.wait_for_message("/joint_states", sensor_msgs.msg.JointState)
        angles = []
        for name in self.joint_names:
          angles.append( msg.position[msg.name.index(name)])
        return angles

    def getArmSeedAngles(self):
        if 'r_shoulder_lift_joint' in self.joint_names:
            return arm_seeds
        else:
            return arm_seeds_without_shoulder
    
        return angles
    def getIK(self, goalPose, link_name, seedAngles):
        """Calls inverse kinematics service for the arm. 
            goalPose -  Pose Stamped message showing desired position of link in coord system
            linkName - goal Pose target link name -  (r_wrist_roll_link or l_wrist_roll_link)
            startAngles -  seed angles for IK
        """
        ikreq = kinematics_msgs.msg.PositionIKRequest()
        ikreq.ik_link_name =link_name
        ikreq.pose_stamped = goalPose
        ikreq.ik_seed_state.joint_state.name = self.joint_names
        ikreq.ik_seed_state.joint_state.position = seedAngles
        ikres  = self._runIK(ikreq, rospy.Duration(5))
        if (ikres.error_code.val != ikres.error_code.SUCCESS): #figure out which error code
            raise rospy.exceptions.ROSException("Could not find IK with " + self.ikPath + "\n\n" + ikres.__str__())
        return ikres

    def goToAngle(self, angles, time, wait = True):
        if (len(angles) != len(self.joint_names)):
            raise Exception("Wrong number of Angles. "+ len(angles) + "given.  "+ len(self.joint_names) + "needed.")
    
        trajMsg = pr2_controllers_msgs.msg.JointTrajectoryGoal()
        
        trajPoint = trajectory_msgs.msg.JointTrajectoryPoint()
        trajPoint.positions= angles
        trajPoint.velocities = [0 for i in range(0, len(self.joint_names))]
        trajPoint.time_from_start = rospy.Duration(time)
            
        trajMsg.trajectory.joint_names = self.joint_names
        trajMsg.trajectory.points.extend([trajPoint])
        self.sendTraj(trajMsg, wait)

    def goToPoint(self, point, frame_id = "torso_lift_link", wait = True):
        pose = makePose(point, [0,0,0,1], frame_id)
        ik  = self.getIK(pose, "r_wrist_roll_link", self.getArmSeedAngles())
        self.goToAngle(ik.solution.joint_state.position, wait)
        
        
    def goToPose(self, pos, orientation, frame_id, time, wait = True):
        """ This method uses getIK to move the x_wrist_roll_link to the 
            specific point and orientation specified in frame_id's coordinates. 
            Time specifies the duration of the planned trajectory, 
            wait whether to block or not. 
        """
        pose = makePose(pos, orientation, frame_id)
        ik  = self.getIK(pose, "r_wrist_roll_link", self.getArmSeedAngles())
        self.goToAngle(ik.solution.joint_state.position, time, wait)

def makePose(xyz, orientation, frameID):
    """This is a shortcut method for making pose stamped messages
    """
    pose = geometry_msgs.msg.PoseStamped()
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = xyz[0], xyz[1], xyz[2]
    pose.pose.orientation.x = orientation[0]
    pose.pose.orientation.y = orientation[1] 
    pose.pose.orientation.z = orientation[2]
    pose.pose.orientation.w = orientation[3]
    pose.header.frame_id = frameID
    pose.header.stamp = rospy.Time.now()
    return pose

---

pr2_dremel_server
Author(s): Christian Bersch, Benjamin Pitzer (maintained by Benjamin Pitzer)
autogenerated on Sun Mar 3 12:07:41 2013