StuweXY Posted August 3, 2017 Share Posted August 3, 2017 Hi guys, I tried to extend the IK Service Example by orienting myself on point #4 of the tutorial to move the robot to the found solution. Everything works, except the moving part. The set_joint_positions function and as well as the move_to_joint_position function return an error AttributeError: 'str' object has no attribute 'set_joint_positions'. I don't see my mistake because in the documentation it says for the function parameter position: positions (dict({str:float})) - joint_name:angle command, and also the tutorial does it like that. So why doesn't it work? Here is my code: #!/usr/bin/env python import pprint import rospy import intera_interface import intera_external_devices from intera_interface import CHECK_VERSION from std_msgs.msg import Header from sensor_msgs.msg import JointState from geometry_msgs.msg import ( PoseStamped, Pose, Point, Quaternion, ) from intera_core_msgs.srv import ( SolvePositionIK, SolvePositionIKRequest, ) class PickAndPlace(object): # Register clean shutdown def clean_shutdown(): print("\nExiting example.") return True def __init__(self): # Verify limb parameters print("Validating limbs...") rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return # Verify robot is enabled print("Getting robot state... ") rs = intera_interface.RobotEnable() print("Enabling robot... ") rs.enable() print("Done.") #Move in neutral position print("Moving to neutral position... ") limb = intera_interface.Limb('right') #limb.move_to_neutral() print("Done.") def doit(self, limb = "right"): # Initialising IK services ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # Defining a new pose poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=0.450628752997, y=0.161615832271, z=0.217447307078, ), orientation=Quaternion( x=0.704020578925, y=0.710172716916, z=0.00244101361829, w=0.00194372088834, ), ), ), } # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') try: rospy.wait_for_service(ns, 5.0) # resp enthaelt momentane joint positions resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return False # Check if result valid, and type of seed ultimately used to get solution if(resp.result_type[0] > 0): # Seed is optional, default mode: each strategy will be used seed_str = { ikreq.SEED_USER: 'User Provided Seed', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp.result_type[0], 'None') rospy.loginfo("SUCCESS\nValid Joint Solution Found from Seed Type:\n%s" %(seed_str,)) # Format solution into Limb API-compatible dictionary limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) #-print("Joints: " + str(tuple(resp.joints[0].position))) # Formatted print of joints (instead of rospy.loginfo()) print("\nIK Joint Solution:") pp = pprint.PrettyPrinter(indent=4) pp.pprint(limb_joints) #rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints) rospy.loginfo("------------------") else: rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") rospy.logerr("Result Error %d", resp.result_type[0]) return False while not rospy.is_shutdown(): print("Moving robot to joint solution...") limb.set_joint_positions(limb_joints) rospy.sleep(0.01) return True if __name__ == '__main__': # Init Node for communication with Master rospy.init_node("PickAndPlace", anonymous=True) # Calling init pickandplace = PickAndPlace() print("--- Initialisation complete. ---") print("Calling doit()...") pickandplace.doit() print("--- Task complete. ---") Link to comment Share on other sites More sharing options...
Recommended Posts
Archived
This topic is now archived and is closed to further replies.