def get_forward_kinematic(joints):
try:
rospy.wait_for_service('compute_fk', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Service call failed:", e)
return None
try:
moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
fk_link = ['base_link', 'tool_link']
joint_names = ['joint_1','joint_2','joint_3','joint_4','joint_5','joint_6']
header = Header(0,rospy.Time.now(),"/world")
rs = RobotState()
rs.joint_state.name = joint_names
rs.joint_state.position = joints
response = moveit_fk(header, fk_link, rs)
except rospy.ServiceException as e:
rospy.logerr("Service call failed:", e)
return(None)
quaternion=[response.pose_stamped[1].pose.orientation.x, response.pose_stamped[1].pose.orientation.y,
response.pose_stamped[1].pose.orientation.z, response.pose_stamped[1].pose.orientation.w]
rpy = get_rpy_from_quaternion(quaternion)
quaternion = Position.Quaternion(round(quaternion[0],3), round(quaternion[1],3), round(quaternion[2],3),
round(quaternion[3],3))
point = Position.Point(round(response.pose_stamped[1].pose.position.x,3), round(response.pose_stamped[1].pose.position.y,3),
round(response.pose_stamped[1].pose.position.z,3))
rpy=Position.RPY(round(rpy[0],3),round(rpy[1],3),round(rpy[2],3))
rospy.loginfo("kinematic forward has been calculated ")
return(point, rpy, quaternion)
def create_position_response(self, status, message, position=None):
position_msg = PositionMessage()
if position != None:
position_msg.name = position.name
position_msg.joints = position.joints
position_msg.rpy = Position.RPY(position.rpy.roll, position.rpy.pitch, position.rpy.yaw)
position_msg.point = Position.Point( position.point.x, position.point.y, position.point.z)
position_msg.quaternion = Position.Quaternion(position.quaternion.x, position.quaternion.y,
position.quaternion.z, position.quaternion.w)
return { 'status': status, 'message': message, 'position': position_msg }
def callback_get_position_list(self, req = None):
pos_list = self.get_all_positions()
msg_list = []
for pos in pos_list:
position_msg = PositionMessage()
position_msg.name = pos.name
position_msg.joints = pos.joints
position_msg.rpy = Position.RPY(pos.rpy.roll, pos.rpy.pitch, pos.rpy.yaw)
position_msg.point = Position.Point( pos.point.x, pos.point.y, pos.point.z)
position_msg.quaternion = Position.Quaternion(pos.quaternion.x, pos.quaternion.y,
pos.quaternion.z, pos.quaternion.w)
msg_list.append(position_msg)
return { 'positions': msg_list }
def callback_manage_position(self, req):
cmd_type = req.cmd_type
position_name = req.position_name
position_msg = req.position
rpy = Position.RPY(position_msg.rpy.roll, position_msg.rpy.pitch, position_msg.rpy.yaw)
point = Position.Point(position_msg.point.x, position_msg.point.y, position_msg.point.z)
quaternion = Position.Quaternion(position_msg.quaternion.x, position_msg.quaternion.y, position_msg.quaternion.z,
position_msg.quaternion.w )
position_data = Position( name = position_msg.name,
joints = position_msg.joints, rpy=rpy, point = point, quaternion = quaternion)
# GET an existing position
if cmd_type == PositionCommandType.GET:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(400, "No position found with name : " + str(position_name))
return self.create_position_response(200, "Position has been found", pos)
# CREATE new position
elif cmd_type == PositionCommandType.CREATE:
(new_position_name, msg) = self.create_new_position(position_data)
if new_position_name == None :
return self.create_position_response(400, msg)
new_position = self.get_position(new_position_name)
if new_position == None :
return self.create_position_response(400, "Failed to create new position")
return self.create_position_response(200, msg , new_position)
# UPDATE existing sequence
elif cmd_type == PositionCommandType.UPDATE:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(400, "No position found with name : " + position_name)
(success, update_msg) = self.update_position(pos, position_data)
if not success:
return self.create_position_response(400, update_msg)
return self.create_position_response(200, update_msg, pos)
# DELETE sequence
elif cmd_type == PositionCommandType.DELETE:
success = self.delete_position(position_name)
if not success:
return self.create_position_response(400, "Could not delete position with name : " + position_name)
return self.create_position_response(200, "Position has been deleted")
# Wrong cmd_type
else:
return self.create_sequence_response(400, "Wrong command type")
quaternion = [
response.pose_stamped[1].pose.orientation.x,
response.pose_stamped[1].pose.orientation.y,
response.pose_stamped[1].pose.orientation.z,
response.pose_stamped[1].pose.orientation.w
]
rpy = get_rpy_from_quaternion(quaternion)
quaternion = Position.Quaternion(round(quaternion[0], 3),
round(quaternion[1], 3),
round(quaternion[2], 3),
round(quaternion[3], 3))
point = Position.Point(round(response.pose_stamped[1].pose.position.x, 3),
round(response.pose_stamped[1].pose.position.y, 3),
round(response.pose_stamped[1].pose.position.z, 3))
rpy = Position.RPY(round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3))
rospy.loginfo("kinematic forward has been calculated ")
return (point, rpy, quaternion)
def get_rpy_from_quaternion(rot):
PI = 3.14159
euler = tf.transformations.euler_from_quaternion(rot)
rpy = [0, 0, 0]
rpy[0] = euler[1] * (-1.0)
rpy[1] = euler[0] - PI / 2.0
rpy[2] = euler[2] - PI / 2.0
# force angle between -PI/PI
for i, angle in enumerate(rpy):
if angle > PI: