← Back to branch summary

~shadowrobot/sr-ros-interface/trunk

« back to all changes in this revision

Viewing changes to sr_move_arm/src/reactive_actions/reactive_grasp.py

  • Committer: Ugo Cupcic
  • Date: 2012-07-27 09:19:40 UTC
  • mfrom: (377.1.1 shadow_robot)
  • Revision ID: ugo@shadowrobot.com-20120727091940-xyoouj843k0kc434
Corrected small issues to get manip stack working with this new stack (missing files and setups)

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_move_arm/src/reactive_actions/reactive_grasp.py
12
12
from sr_robot_msgs.srv import which_fingers_are_touching
13
13
 
14
14
from actionlib import SimpleActionClient
15
 
from motion_planning_msgs.msg import *
 
15
#from motion_planning_msgs.msg import *
16
16
from arm_navigation_msgs.msg import *
17
 
from geometric_shapes_msgs.msg import Shape
 
17
#from geometric_shapes_msgs.msg import Shape
18
18
from actionlib_msgs.msg import GoalStatus
19
19
 
20
20
##abort exception
64
64
 
65
65
        #client to the tactile sensor manager.
66
66
        rospy.loginfo("Waiting for service which_fingers_are_touching")
67
 
        rospy.wait_for_service('/sr_tactile_v/which_fingers_are_touching')
 
67
        rospy.wait_for_service('which_fingers_are_touching')
68
68
        rospy.loginfo("OK service which_fingers_are_touching found.")
69
 
        self.which_fingers_are_touching_client = rospy.ServiceProxy('/sr_tactile_v/which_fingers_are_touching', which_fingers_are_touching)
 
69
        self.which_fingers_are_touching_client = rospy.ServiceProxy('which_fingers_are_touching', which_fingers_are_touching)
70
70
 
71
71
        #load tactile thresholds
72
 
        self.light_touch_thresholds = rospy.get_param('~light_touch_thresholds', [100.,100.,100.,100.,0.0])
73
 
        self.grasp_touch_thresholds = rospy.get_param('~grasp_touch_thresholds', [117.,117.,113.,111.,0.0])
 
72
        self.light_touch_thresholds = rospy.get_param('light_touch_thresholds', [100.,100.,100.,100.,0.0])
 
73
        rospy.loginfo("light and grasp threashold are :")
 
74
        rospy.loginfo(self.light_touch_thresholds)
 
75
        self.grasp_touch_thresholds = rospy.get_param('grasp_touch_thresholds', [117.,117.,113.,111.,0.0])
 
76
        rospy.loginfo(self.grasp_touch_thresholds)
74
77
 
75
78
        #dictionary for ManipulationPhase
76
79
        self.manipulation_phase_dict = {}
354
357
                    touching = fingers_touching[touch_index]
355
358
                if touching == 0:
356
359
                    joint_target = pregrasp_target + float(grasp_target - pregrasp_target)*(float(i_step) / float(nb_steps) )
 
360
                    myjoint_target = joint_target * 180.0 / math.pi
357
361
                    current_targets[index] = joint_target
358
 
                    sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target))
359
 
                    rospy.logdebug("["+joint_name+"]: (p/g/t) = "+str(pregrasp_target)+"/"+str(grasp_target)+"/"+str(joint_target) + " ("+
 
362
                    sendupdate_msg.append(joint(joint_name = joint_name, joint_target = myjoint_target))
 
363
                    rospy.logdebug("["+joint_name+"]: (p/g/t) = "+str(pregrasp_target)+"/"+str(grasp_target)+"/"+str(myjoint_target) + " ("+
360
364
                                   str(float(i_step) / float(nb_steps))+"%)")
361
365
 
362
366
            self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
459
463
        sendupdate_msg = []
460
464
 
461
465
        for (joint_name, joint_target) in zip(joint_names, joint_targets):
462
 
            sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target))
 
466
            myjoint_target=joint_target * 180 / math.pi
 
467
            sendupdate_msg.append(joint(joint_name = joint_name, joint_target = myjoint_target))
463
468
 
464
469
        self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
465
470