Extended IK Service Example

[StuweXY]

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. ---")