def position_api_joint_space_handler(req):
rospy.loginfo("Inside PositionAPIJointSpace Service call!")
'''
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()
# Form a list of the target joint angles:
targetJointAngles = [req.joint1, req.joint2, req.joint3, req.joint4, req.joint5, req.joint6]
rospy.loginfo("Joint Angle saved")
'''
Encapsulate the target jointangles into another list (this forms 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/joint_trajectory_controller/follow_joint_trajectory'
rospy.loginfo("Trajectory created")
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 2
else:
print("Action server ready!")
rospy.loginfo("Creating message")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
rospy.loginfo("Sending message to client")
ah.set_client(client)
ah.wait_inside()
# return ah
return 0
def powerball_remote():
ah = simple_script_server.action_handle("move", "arm", "home", False, False)
if False:
return ah
else:
ah.set_active()
# Ask the user to input the desired joint angles:
print("Joint 1 is the proximal joint (the base), Joint 6 is the distal joint (the wrist)")
joint1 = float(input("Please enter the desired angle for Joint 1: "))
joint2 = float(input("Please enter the desired angle for Joint 2: "))
joint3 = float(input("Please enter the desired angle for Joint 3: "))
joint4 = float(input("Please enter the desired angle for Joint 4: "))
joint5 = float(input("Please enter the desired angle for Joint 5: "))
joint6 = float(input("Please enter the desired angle for Joint 6: "))
# This list holds the joint angles:
jointAngles = [joint1, joint2, joint3, joint4, joint5, joint6]
# Encapsulate the jointAngles in another list (the trajectory):
traj = [jointAngles]
# Generate the trajectory message to send:
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
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 message to the action server:
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!")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def position_api_joint_space_handler(req):
'''
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()
# Form a list of the target joint angles:
targetJointAngles = [req.joint1, req.joint2, req.joint3, req.joint4, req.joint5, req.joint6]
'''
Encapsulate the target jointangles into another list (this forms 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!")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
# return ah
return 0
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
def position_api_coord_space_quat_handler(req):
#rospy.loginfo("Inside PostionAPICoordSpaceQuat Service call!")
global jointStateCallbackEx
global gripper
gripper=0.5
'''
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)
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:
if len(jointAngles) != 0:
# We have a valid solution! Move the Powerball to this location:
# Generate the trajectory message to send to the Powerball:
# End code simulation
# Code real robot
#targetJointAngles = targetJointAngles[:6]
jointAngles.append(req.gripper)
#trajectory=jointAngles
#trajectory.append(1.0)
# Encapsulate the targetJointAngles into a trajectory:
#traj = [targetJointAngles]
traj = [jointAngles]
#traj.append(gripper)
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','pg70_finger_left_joint']
point_nr = 0
for point in traj:
print(point)
#Position only in simulation
#position.append((point))
point_nr += 1
print("Point num %d " % point_nr)
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0.0, 0.0, 0.0 ,0.0, 0.0 ,0.0, 0.1]
point_msg.time_from_start = rospy.Duration(3 * point_nr)
traj_msg.points.append(point_msg)
action_server_name = '/arm/joint_trajectory_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
return 0
else:
print("Action server ready for Coordinate API Request")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
print("goal send")
#ah.set_client(client)
#ah.wait_inside()
return 1
def powerball_remote():
ah = simple_script_server.action_handle("move", "arm", "home", False,
False)
if False:
return ah
else:
ah.set_active()
# Ask the user to input the desired joint angles:
print(
"Joint 1 is the proximal joint (the base), Joint 6 is the distal joint (the wrist)"
)
joint1 = float(input("Please enter the desired angle for Joint 1: "))
joint2 = float(input("Please enter the desired angle for Joint 2: "))
joint3 = float(input("Please enter the desired angle for Joint 3: "))
joint4 = float(input("Please enter the desired angle for Joint 4: "))
joint5 = float(input("Please enter the desired angle for Joint 5: "))
joint6 = float(input("Please enter the desired angle for Joint 6: "))
# This list holds the joint angles:
jointAngles = [joint1, joint2, joint3, joint4, joint5, joint6]
# Encapsulate the jointAngles in another list (the trajectory):
traj = [jointAngles]
# Generate the trajectory message to send:
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
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 message to the action server:
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!")
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
