def position_api_coord_space_quat_handler(req):
global jointStateCallbackEx
'''
This simple_script_server is a custom library that was created by
the Fraunhofer institute. An action_handle will listen for position commands.
'''
ah = simple_script_server.action_handle("move", "arm", "home", False, False)
if False:
return ah
else:
ah.set_active()
# Get the target (X, Y, Z) coordinates to move to:
targetCoords = [req.position.x, req.position.y, req.position.z]
# Get the desired rotation (in quaternion) to move to:
targetRot = [req.orientation.w, req.orientiation.x, req.orientation.y, req.orientation.z]
'''
Currently the Powerball requires a list of the 6 target joint angles to move.
We can calculate these target joint angles by calling the inverse kinematics
functions:
'''
'''
First, get a list of the current joint angles. The joint angles can be
found from rostopic /joint_states
'''
sub = rospy.Subscriber("/joint_states", JointState, jointStateCallback)
while jointStateCallbackEx == False:
pass
rospy.Subscriber.unregister(sub)
jointStateCallbackEx = False
# We need to convert the quaternion into a 4x4 homogeneous transformation matrix:
homoMat = quaternion_matrix(targetRot)
# Insert the desired target joint coordinate into the transformation matrix:
homoMat[0,3] = req.xCoord
homoMat[1,3] = req.yCoord
homoMat[2,3] = req.zCoord
'''
Calculate the inverse kinematics given the target rotation/position and
the list of current joint angles:
'''
targetJointAngles = kf.ikine(homoMat, jointAngles)
if len(targetJointAngles) != 0:
# We have a valid solution! Move the Powerball to this location:
targetJointAngles = targetJointAngles[:6]
# Encapsulate the targetJointAngles into a trajectory:
traj = [targetJointAngles]
# Generate the trajectory message to send to the Powerball:
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
traj_msg.joint_names = ['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint']
point_nr = 0
# Set the target velocities of the target joints. They are set to 0 to denote stopping at the destinations:
for point in traj:
point_nr += 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0] * 6
point_msg.time_from_start = rospy.Duration(3 * point_nr)
traj_msg.points.append(point_msg)
# Send the position control message to the action server node:
action_server_name = '/arm_controller/follow_joint_trajectory'
client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
if not client.wait_for_server(rospy.Duration(5)):
print("Action server not ready within timeout. Aborting...")
ah.set_failed(4)
return ah
else:
print("Action server ready for Coordinate API Request")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return 0
def position_api_coord_space_quat_handler(req):
#rospy.loginfo("Inside PostionAPICoordSpaceQuat Service call!")
global jointStateCallbackEx
#global joint_msg_leap
#joint_msg_leap.name=['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint','base_joint_gripper_left','base_joint_gripper_right']
#joint_msg_leap.position=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
#joint_msg_leap.velocity=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
#joint_msg_leap.effort=[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
#rospy.loginfo("Inside server_callback!")
'''
This simple_script_server is a custom library that was created by
the Fraunhofer institute. An action_handle will listen for position commands.
'''
ah = simple_script_server.action_handle("move", "arm", "home", False, False)
if False:
rospy.loginfo("Action server ready!")
return ah
else:
ah.set_active()
#rospy.loginfo("ah active!")
# Get the target (X, Y, Z) coordinates to move to:
targetCoords = [req.target.position.x, req.target.position.y, req.target.position.z]
#rospy.loginfo("TargetCoords read!")
# Get the desired rotation (in quaternion) to move to:
targetRot = [req.target.orientation.w, req.target.orientation.x, req.target.orientation.y, req.target.orientation.z]
#rospy.loginfo("TargetRot read!")
'''
Currently the Powerball requires a list of the 6 target joint angles to move.
We can calculate these target joint angles by calling the inverse kinematics
functions:
'''
'''
First, get a list of the current joint angles. The joint angles can be
found from rostopic /joint_states
'''
sub = rospy.Subscriber("/joint_states", JointState, jointStateCallback,queue_size=1)
pub = rospy.Publisher("joint_leap", JointState, queue_size=10)
while jointStateCallbackEx == False:
pass
rospy.Subscriber.unregister(sub)
jointStateCallbackEx = False
# We need to convert the quaternion into a 4x4 homogeneous transformation matrix:
homoMat = quaternion_matrix(targetRot)
# Insert the desired target joint coordinate into the transformation matrix:
homoMat[0,0] = 1
homoMat[1,0] = 0
homoMat[2,0] = 0
homoMat[0,1] = 0
homoMat[1,1] = 1
homoMat[2,1] = 0
homoMat[0,2] = 0
homoMat[1,2] = 0
homoMat[2,2] = 1
homoMat[0,3] = req.target.position.x
homoMat[1,3] = req.target.position.y
homoMat[2,3] = req.target.position.z
#RotxMat=numpy.array([[1,0,0,0],[0,0,1,0],[0,-1,0,0],[0,0,0,1]])
#RotyMat=numpy.array([[0,0,-1,0],[0,1,0,0],[1,0,0,0],[0,0,0,1]])
#SolMat=numpy.array([[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0]])
#SolMat=RotyMat*RotxMat*homoMat
'''
Calculate the inverse kinematics given the target rotation/position and
the list of current joint angles:
'''
#print("homoMat is: ")
#print(homoMat)
#print(SolMat)
#print(jointAngles)
try:
targetJointAngles = kf.ikine(homoMat, jointAngles)
except ValueError:
print("ERROR: Position out of Range")
return 0
if len(targetJointAngles) != 0:
# We have a valid solution! Move the Powerball to this location:
# Code for Simulation
#print(targetJointAngles)
# Encapsulate the targetJointAngles into a trajectory:
traj = []
joint1=targetJointAngles[0]
joint1=targetJointAngles[0]
joint2=targetJointAngles[1]
joint3=targetJointAngles[2]
joint4=targetJointAngles[3]
joint5=targetJointAngles[4]
joint6=targetJointAngles[5]
traj.append(joint1)
traj.append(joint2)
traj.append(joint3)
traj.append(joint4)
traj.append(joint5)
traj.append(joint6)
#print(traj)
position = []
# Generate the trajectory message to send to the Powerball:
# End code simulation
#Code real robot
#targetJointAngles = targetJointAngles[:6]
# Encapsulate the targetJointAngles into a trajectory:
#traj = [targetJointAngles]
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
traj_msg.joint_names = ['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint']
point_nr = 0
# Set the target velocities of the target joints. They are set to 0 to denote stopping at the destinations:
for point in traj:
#print(point)
#Position only in simulation
position.append((point))
point_nr += 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0] * 6
point_msg.time_from_start = rospy.Duration(3 * point_nr)
traj_msg.points.append(point_msg)
# Send the position control message to the action server node:
position.append(gripperPos[0])
obj1.position=position
#rospy.loginfo(obj1.position[0])
#rospy.loginfo(obj1.position[1])
#rospy.loginfo(obj1.position[2])
#rospy.loginfo(obj1.position[3])
#rospy.loginfo(obj1.position[4])
#rospy.loginfo(obj1.position[5])
#rospy.loginfo(obj1.position[6])
#rospy.loginfo(obj1.position[7])
resp = PositionAPICoordSpaceQuatResponse()
resp.ret=1;
resp.joint1=obj1.position[0]
resp.joint2=obj1.position[1]
resp.joint3=obj1.position[2]
resp.joint4=obj1.position[3]
resp.joint5=obj1.position[4]
resp.joint6=obj1.position[5]
#action_server_name = '/arm/joint_trajectory_controller/follow_joint_trajectory'
#pub.publish(obj1)
#client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
#if not client.wait_for_server(rospy.Duration(5)):
# print("Action server not ready within timeout. Aborting...")
# ah.set_failed(4)
#return ah
# return 0
#else:
#print("Action server ready for Coordinate API Request")
#client_goal = FollowJointTrajectoryGoal()
#client_goal.trajectory = traj_msg
#client.send_goal(client_goal)
#ah.set_client(client)
#ah.wait_inside()
return resp
