$search

dremel.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# Copyright (c) 2010, Bosch LLC
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of Bosch LLC nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Authors: Christian Bersch, Bosch LLC
#          




import roslib; roslib.load_manifest("pr2_dremel_server")
import actionlib
import rospy
import copy
import pr2_controllers_msgs.msg
from pr2_controller_manager import pr2_controller_manager_interface

import geometry_msgs.msg
import kinematics_msgs.msg
import kinematics_msgs.srv
import arm_navigation_msgs.msg
import arm_navigation_msgs.srv
import trajectory_msgs.msg 
from std_msgs.msg import *
from sensor_msgs.msg import *
from math import *
import numpy as np
import tf
import tf.listener
import threading
from simple_robot_control.srv import ReturnJointStates


from controller_interface import *

import simple_robot_control 
import numpy

import pr2_dremel_server.msg as action_msg




force = 3.0
#quats_default = (0.009, -0.022, 0.018, 1.000)

#This are the parameters for the table top plane position and orientation.
#These can be retrieved by placing the dremel tool parallel to and slightly above the work piece.
#Run rosrun  tf tf_echo /torso_lift_link /r_gripper_tool_frame for obtaining these values


quats_default = ( 0.003, -0.017, -0.018, 1.000)
table_height_default = -0.245


table_height = table_height_default
quats = quats_default


def scale_pts(points):
    min_x = min(points[:,0])
    max_x = max(points[:,0])
    
    min_y = min(points[:,1])
    max_y = max(points[:,1])
    print "max_y", max_y
    print "min_y", min_y    
    points[:,0] -= 0.5 * (max_x + min_x)
    points[:,1] -= 0.5 * (max_y + min_y)
    pts_temp = copy.deepcopy(points[:,1])
    points[:,1] = points[:,0]
    points[:,0] = pts_temp
    points[:,0] *= -1.0
    points[:,1] *= -1.0
    print "zeroed pts", points    
    scale_factor = 0.08 / (max_y - min_y)
    points[:,0] *=  scale_factor
    points[:,1] *=  scale_factor
    
    points[:,0] +=  0.55
    
    return points
    
        

max_vel = 0.002


class DremelPlotter:
    def __init__(self):
      rospy.set_param("r_arm_subset_controller", rospy.get_param("r_arm_controller"))
      rospy.set_param("r_arm_subset_controller/joints", r_arm_joints_without_shoulder)
      
      quats = rospy.get_param("~gripper_orientation", quats_default)
      table_height = rospy.get_param("~table_torso_z", table_height_default)
      print "table_height", table_height
      print 'orienta', quats
    #  rospy.set_param
    
      self.shoulder_controller = JointForceController("r_shoulder_lift_joint")
      self.arm_position = ArmPositionController("r_arm_controller")
      self.arm_subset_position = ArmPositionController("r_arm_subset_controller")
    
      self.arm_subset_position.load()
    
    
     
      self.arm = Arm("/r_arm_controller", "/pr2_right_arm_kinematics")
 #     self.arm_subset = Arm("/r_arm_subset_controller", "/pr2_right_arm_kinematics")
      
      self.listener = tf.TransformListener()
      
      self.force_thresh = 0
      self.current_table_z = 0.0
      self.reposition_offset_x = 0.002
      

      
      
      
      #actionserver stuff
      self._feedback = action_msg.CarveSegmentsFeedback()
      self._result   = action_msg.CarveSegmentsResult()
      self._as = None
      
    def startActionServer(self):
        self._as = actionlib.SimpleActionServer("dremel_actionserver", action_msg.CarveSegmentsAction, execute_cb=self.plot_CB, auto_start = False)
        self._as.start()
        
      
    def joint_states_callback(self, msg):
      #  print 'in js callback msg:'
        self.lock.acquire()
        self.name = msg.name
        self.position = msg.position
        self.velocity = msg.velocity
        self.effort = msg.effort
        self.lock.release()
        rospy.sleep(0.01)
    
        #thread function: listen for joint_states messages
    def joint_states_listener(self):
        rospy.Subscriber('joint_states', JointState, self.joint_states_callback)
        rospy.spin()
        
    def getJointEfforts(self):
        efforts = []
        self.lock.acquire()
        all_name = self.name[:]
        all_effort = self.effort[:]
        self.lock.release()
        for cur_name in self.arm.joint_names:
            if cur_name in all_name:
                index = all_name.index(cur_name)
                efforts.append(all_effort[index])
        print 'efforts : ', efforts
        return efforts
    
    def getJointEffortsNT(self):
        msg = rospy.wait_for_message("joint_states", JointState)
        efforts = []
        for cur_name in self.arm.joint_names:
            if cur_name in msg.name:
                index = msg.name.index(cur_name)
                efforts.append(msg.effort[index])
        print 'efforts : ', efforts
        return efforts
      
      
    def approach(self, point): #postion to force
        self.shoulder_controller.stop()
        self.arm_subset_position.stop()
        self.arm_position.start()
        (trans_l,rot_l) = self.listener.lookupTransform( 'torso_lift_link', 'r_wrist_roll_link', rospy.Time(0))
        print "current pos", trans_l[0], trans_l[1], trans_l[2]
        print "approach point", point[0], point[1], point[2]

        hit_wood = False
        pos = point[:]
        num_steps = 0
        while not hit_wood:
            current_efforts = self.getJointEffortsNT()
            if current_efforts[1] > 0.0:
                print "hit wood!"
                hit_wood = True
                pos[2] = (num_steps + 1) * -0.002 + trans_l[2] 
                self.arm.goToPose(pos, quats, "torso_lift_link",0.5)            
                self.current_table_z = pos[2]

            else:
                num_steps = num_steps + 1
                pos = point[:]
                pos[2] = num_steps * -0.002 + trans_l[2] 
                self.arm.goToPose(pos, quats, "torso_lift_link",0.5)
      

        self.arm_position.stop()
        self.shoulder_controller.start()
        self.arm_subset_position.start()
        rospy.sleep(1)
        print "approach done and in force control"


        

            
    def reposition(self, point): #force to position
        print "repostion"
        (trans_l,rot_l) = self.listener.lookupTransform( 'torso_lift_link', 'r_wrist_roll_link', rospy.Time(0))
        self.arm_subset_position.stop()
        self.shoulder_controller.stop()  
        self.arm_position.start()
        trans_l = list(trans_l)
        trans_l[0] += self.reposition_offset_x
        trans_l[2] += 0.004
        self.arm.goToPose(trans_l, quats, "torso_lift_link",0.5)
        point[0] += self.reposition_offset_x
        self.arm.goToPose(point, quats, "torso_lift_link", 2)
        print 'reposition done'
            
        
        
    def plot_CB(self, goal):
        approach_offset = 0.025

        print "goal", goal
        self.num_segments = len(goal.segments)
        self.segs_completed = 0
        pts = []
        for seg in goal.segments:
            if self.num_segments > 0:
                first_point = [seg.points[0].x,seg.points[0].y, approach_offset]
                pts.append(first_point)
            for p in seg.points[:-1]:
                point = [p.x, p.y, 0.0]
                pts.append(point)
            if self.num_segments > 1:
                last_point = [seg.points[-2].x,seg.points[-2].y, approach_offset]
                pts.append(last_point)
        
        points = numpy.array(pts)
        points[:,2] += table_height
        
        print "points", points
        
        points = scale_pts(points)
        print "points", points
        
        
        
        self.shoulder_controller.stop()
        self.arm_subset_position.stop()
        self.arm_position.start()
        
        
        approach_z = points[0][2]
        print "approach_z", approach_z
        
        #go to first point 
        first_point  = points[0]
        prev = first_point[:]
        first_point[2] +=0.03
        first_pose = makePose(first_point, quats, "torso_lift_link")
        current_angles = self.arm.getArmSeedAngles()
        ik  = self.arm.getIK(first_pose, "r_wrist_roll_link", current_angles)
        self.arm.goToAngle(ik.solution.joint_state.position, 3.0)
        
        force_control = False
        last_command = None
        success = True
        
        for p in points:
            if self._as.is_preempt_requested():
                rospy.loginfo('Preempted')
                self._as.set_preempted()
                success = False
                break
            print "at point", p[0],p[1],p[2]
            print "table_z", self.current_table_z  
            if p[2] < approach_z:
                force_control = True
                p[2] = self.current_table_z 
            else:
                force_control = False
        
            dist = numpy.linalg.norm(prev - p)
            max_time = 0.1 + dist / max_vel
            max_time = min(max_time, 10.0)
            rospy.loginfo("%f %f", dist, max_time)
            pose = makePose(p, quats , "torso_lift_link")
            current_angles = self.arm.getArmSeedAngles()
            ik  = self.arm.getIK(pose, "r_wrist_roll_link", current_angles)
            subset_joints = self.arm_subset.getJointNames()
            angles = []
            for joint in subset_joints:
                angles.append(ik.solution.joint_state.position[ ik.solution.joint_state.name.index(joint) ] )
    

    
            shoulder_position = ik.solution.joint_state.position[ ik.solution.joint_state.name.index("r_shoulder_lift_joint") ]
    
            if force_control != last_command:
                if force_control: # approaching, so we move elbow last from position control to froce control
                    print 'approaching'
                    self.approach(p)
                    print 'force elbow'
                    self.shoulder_controller.force(force)
                    print 'force elbow done'
                    rospy.sleep(1)
                else: # pulling away, so we move elbow first - from force control to position control
                    print 'pulling away and position to', p
                    self.reposition(p)
                    self.segs_completed = self.segs_completed + 1
                    self._feedback.percent_complete = (float) (self.segs_completed) / self.num_segments
                    self._as.publish_feedback(self._feedback)
                    rospy.sleep(2)

            else: # just go to the next position 
                if not force_control:  #staying in position control
                    print 'repositioning to point', p
                    self.arm.goToPose(p, quats, "torso_lift_link",2.0)

                else:
                    print 'force drawing to angles', angles
                    self.arm_subset.goToAngle(angles, max_time) #staying in froce control
                    print 'force drawing done'
             
            last_command = force_control
            prev = p
      
        rospy.sleep(2)
        self.shoulder_controller.stop()
        self.arm_subset_position.stop()
        self.arm_position.start()
      
      
        prev[2] +=0.05
        last_pose = makePose(prev, quats, "torso_lift_link")
        ik  = self.arm.getIK(last_pose, "r_wrist_roll_link", self.arm.getArmSeedAngles())
        self.arm.goToAngle(ik.solution.joint_state.position, 3.0)
        
        if success:
            self._as.set_succeeded(self._result)
        



if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('dremel_plotter_actionserver')
        
        dremel = DremelPlotter()
        dremel.startActionServer()
        rospy.spin()

    except rospy.ROSInterruptException:
        print "program interrupted before completion"

---

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