def get_forward_kinematics(self, joints):
"""
Get forward kinematic from joints
:param joints: list of joints value
:type joints: Pose
:return: A RobotState object
"""
try:
rospy.wait_for_service('compute_fk', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Arm commander - Impossible to connect to FK service : " + str(e))
return RobotState()
try:
moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
fk_link = ['base_link', 'tool_link']
header = Header(0, rospy.Time.now(), "world")
rs = RobotStateMoveIt()
rs.joint_state.name = self.__arm_state.joints_name
rs.joint_state.position = joints
response = moveit_fk(header, fk_link, rs)
except rospy.ServiceException as e:
rospy.logerr("Arm commander - Failed to get FK : " + str(e))
return RobotState()
pose = self.__transform_handler.ee_link_to_tcp_pose_target(response.pose_stamped[1].pose, "tool_link")
quaternion = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rpy = euler_from_quaternion(quaternion)
quaternion = (round(quaternion[0], 3), round(quaternion[1], 3), round(quaternion[2], 3),
round(quaternion[3], 3))
point = (round(pose.position.x, 3),
round(pose.position.y, 3),
round(pose.position.z, 3))
return RobotState(position=Point(*point), rpy=RPY(*rpy), orientation=Quaternion(*quaternion))
def _get_new_joints_w_ik(self, shift_command):
quat_jog = quaternion_from_euler(shift_command[3], shift_command[4],
shift_command[5])
quat_target = quaternion_multiply(quat_jog, [
self._last_robot_state_published.orientation.x,
self._last_robot_state_published.orientation.y,
self._last_robot_state_published.orientation.z,
self._last_robot_state_published.orientation.w
])
rpy_target = RPY(*euler_from_quaternion(quat_target))
self._new_robot_state = RobotState()
self._new_robot_state.position.x = self._last_robot_state_published.position.x + shift_command[
0]
self._new_robot_state.position.y = self._last_robot_state_published.position.y + shift_command[
1]
self._new_robot_state.position.z = self._last_robot_state_published.position.z + shift_command[
2]
self._new_robot_state.orientation = Quaternion(*quat_target)
self._new_robot_state.rpy = rpy_target
self.__validate_params_pose(self._new_robot_state)
success, joints = self.__kinematics_handler.get_inverse_kinematics(
Pose(self._new_robot_state.position,
self._new_robot_state.orientation))
return success, joints
def get_workspace_poses(self, name):
ws_raw = self.__ws_manager.read(name)
poses = [
RobotState(position=Point(*pose[0]), rpy=RPY(*pose[1]))
for pose in ws_raw.robot_poses
]
return poses
def __callback_get_workspace_poses(self, req):
try:
poses = self.get_workspace_poses(req.name)
return CommandStatus.SUCCESS, "Success", poses
except Exception as e:
return CommandStatus.POSES_HANDLER_READ_FAILURE, str(
e), 4 * [RobotState()]
def __init__(self):
super(HoldingRegisterDataBlock, self).__init__()
self.execution_thread = threading.Thread()
self.is_action_client_running = False
self.cmd_action_client = None
# Tool
self.current_tool_id = None
self.list_id_grippers = [11, 12, 13, 14]
rospy.Subscriber('/niryo_robot_tools_commander/current_id', Int32,
self.sub_selected_tool_id)
# Variables
self.workspace_name = None
self.vision_color = None
self.vision_shape = None
self.height_offset = None
self.x_rel = None
self.y_rel = None
self.yaw_rel = None
# List of enums names for color and type that are available in vision package
self.list_enum_color = [color_obj.name for color_obj in ColorHSV]
self.list_enum_type = [type_obj.name for type_obj in ObjectType]
# List of enums values for color and shape that are available for modbus client
self.list_enum_color_nb = [color_obj.value for color_obj in ColorRequest]
self.list_enum_shape_nb = [shape_obj.value for shape_obj in ShapeRequest]
self.null_robot_state = RobotState()
def __callback_target_pose(self, req):
try:
pose = self.get_target_pose(req.workspace, req.height_offset,
req.x_rel, req.y_rel, req.yaw_rel)
return CommandStatus.SUCCESS, "Success", pose
except Exception as e:
return CommandStatus.POSES_HANDLER_COMPUTE_FAILURE, str(
e), RobotState()
def save_workspace(name, list_poses_raw):
list_poses = []
for p in list_poses_raw:
point = Point(*p[0])
rpy = RPY(*p[1])
list_poses.append(RobotState(point, rpy, Quaternion()))
req = ManageWorkspaceRequest()
req.cmd = ManageWorkspaceRequest.SAVE
req.workspace.name = name
req.workspace.poses = list_poses
return call_service('/niryo_robot_poses_handlers/manage_workspace', ManageWorkspace, req)
def __publish_state(self, _):
self.__update_ee_link_pose()
msg = RobotState()
msg.position.x = self.__position[0]
msg.position.y = self.__position[1]
msg.position.z = self.__position[2]
msg.rpy.roll = self.__rpy[0]
msg.rpy.pitch = self.__rpy[1]
msg.rpy.yaw = self.__rpy[2]
msg.orientation.x = self.__quaternion[0]
msg.orientation.y = self.__quaternion[1]
msg.orientation.z = self.__quaternion[2]
msg.orientation.w = self.__quaternion[3]
msg.twist = self.__twist
msg.tcp_speed = self.__tcp_speed
self.__arm_state.robot_state = msg
try:
self.__robot_state_publisher.publish(msg)
except rospy.ROSException:
return
def __callback_target_pose(self, req):
try:
if req.grip == req.GRIP_AUTO:
pose = self.get_target_pose_auto_grip(req.workspace,
req.tool_id,
req.height_offset,
req.x_rel, req.y_rel,
req.yaw_rel)
else:
pose = self.get_target_pose(req.workspace, req.grip,
req.height_offset, req.x_rel,
req.y_rel, req.yaw_rel)
return CommandStatus.SUCCESS, "Success", pose
except Exception as e:
return CommandStatus.POSES_HANDLER_COMPUTE_FAILURE, str(
e), RobotState()
def get_target_pose(self, workspace, height_offset, x_rel, y_rel, yaw_rel):
"""
Computes the robot pose that can be used to grab an object which is
positioned relative to the given workspace.
:param workspace: name of the workspace the object is in
:param height_offset: z-offset that is added
:param x_rel: x relative position of the object inside working zone
:param y_rel: y relative position of the object inside working zone
:param yaw_rel: angle of the object inside working zone
"""
current_ws = self.__ws_manager.read(workspace)
self.__transform_handler.set_relative_pose_object(
current_ws,
x_rel,
y_rel,
yaw_rel,
yaw_center=current_ws.yaw_center)
if self.__tcp_enabled:
base_link_to_tool_target = self.__transform_handler.get_object_transform(
z_off=height_offset)
else:
current_grip = self.__grip_manager.read(
self.__tool_id_gripname_dict[self.__tool_id])
current_grip.transform.transform.translation.z += height_offset
self.__transform_handler.set_grip(current_grip)
base_link_to_tool_target = self.__transform_handler.get_gripping_transform(
)
q = base_link_to_tool_target.transform.rotation
roll, pitch, yaw = euler_from_quaternion([q.x, q.y, q.z, q.w])
pose = RobotState()
pose.position.x = base_link_to_tool_target.transform.translation.x
pose.position.y = base_link_to_tool_target.transform.translation.y
pose.position.z = base_link_to_tool_target.transform.translation.z
pose.rpy.roll = roll
pose.rpy.pitch = pitch
pose.rpy.yaw = yaw
return pose
def _get_new_joints_w_ik(self, shift_command):
self._new_robot_state = RobotState()
self._new_robot_state.position.x = self._last_robot_state_published.position.x + shift_command[
0]
self._new_robot_state.position.y = self._last_robot_state_published.position.y + shift_command[
1]
self._new_robot_state.position.z = self._last_robot_state_published.position.z + shift_command[
2]
self._new_robot_state.rpy.roll = self._last_robot_state_published.rpy.roll + shift_command[
3]
self._new_robot_state.rpy.pitch = self._last_robot_state_published.rpy.pitch + shift_command[
4]
self._new_robot_state.rpy.yaw = self._last_robot_state_published.rpy.yaw + shift_command[
5]
self._new_robot_state.orientation = Quaternion()
self.__validate_params_pose(self._new_robot_state)
response = self._service_ik(self._new_robot_state)
return response.success, response.joints
def __init__(self, arm_state):
self.__arm_state = arm_state
# - Values Init
self._shift_mode = None
self.__collision_detected = False
self._last_target_values = [0.0 for _ in range(6)]
self._last_shift_values_cmd = None
self._target_lock = Lock()
self._target_values = None
self._current_jogged_joints = []
self.__error_tolerance_joint = rospy.get_param(
"~error_tolerance_joint")
self.__time_without_jog_limit = rospy.get_param(
"~time_without_jog_TCP_limit"
) # jog disabled after one second for the jogTCP Niryo Studio
# - Move It Commander / Get Arm MoveGroupCommander
self.__arm = self.__arm_state.arm
# - Kinematics
self.__kinematics_handler = self.__arm_state.kinematics
# Validation
self.__parameters_validator = self.__arm_state.parameters_validator
jog_limits = rospy.get_param("~jog_limits")
self.__pose_translation_max = jog_limits["translation"]
self.__pose_rotation_max = jog_limits["rotation"]
self.__joints_rotation_max = jog_limits["joints"]
self._new_robot_state = RobotState()
self._last_robot_state_published = RobotState()
# - Publisher which publishes if JogController is enabled
self._enabled = False
self.__jog_errors_cpt = 0
self._jog_enabled_publisher = rospy.Publisher(
'/niryo_robot/jog_interface/is_enabled', Bool, queue_size=3)
self._jog_errors_publisher = rospy.Publisher(
'/niryo_robot/jog_interface/errors', String, queue_size=3)
rospy.Timer(
rospy.Duration(1.0 / rospy.get_param("~jog_enable_publish_rate")),
self._publish_jog_enabled)
self._check_disable_jog_timer = None
self._last_command_timer = rospy.get_time()
self._publish_jog_enabled()
# - Direct publisher to joint controller
self._joint_trajectory_publisher = rospy.Publisher(
rospy.get_param("~joint_controller_name") + '/command',
JointTrajectory,
queue_size=3)
# Publishing rate
self._timer_rate = rospy.get_param("~jog_timer_rate_sec")
self._publisher_joint_trajectory_timer = None
# - Subscribers
# - Joint controller state, used to check collisions
rospy.Subscriber(
rospy.get_param("~joint_controller_name") + '/state',
JointTrajectoryControllerState,
self.__callback_joint_controller_state)
# topic used to jog pose via Niryo Studio, to avoid calling the service
self._jog_command_ik = None
rospy.Subscriber('/niryo_robot_arm_commander/send_jog_command_ik',
CommandJog,
self.__callback_send_jog_command_ik,
queue_size=20)
# topic used to jog joints via Niryo Studio, to avoid calling the service
rospy.Subscriber('/niryo_robot_arm_commander/send_jog_joints_command',
CommandJog,
self.__callback_send_jog_joints_command,
queue_size=10)
# - Service
# Service to enable Jog Controller
rospy.Service('/niryo_robot/jog_interface/enable', SetBool,
self.__callback_enable_jog)
rospy.Service('/niryo_robot/jog_interface/jog_shift_commander',
JogShift, self.__callback_jog_commander)
def __callback_jog_commander(self, msg):
# check if the learning mode is false and the robot is calibrated
self.__check_before_use_jog()
shift_mode = msg.cmd
if shift_mode != self._shift_mode:
self._reset_last_pub()
self._shift_mode = shift_mode
shift_command = list(msg.shift_values)
if shift_mode == JogShiftRequest.POSE_SHIFT:
shift_command[:3] = [
min([v, math.copysign(self.__pose_translation_max, v)],
key=abs) for v in shift_command[:3]
]
shift_command[3:] = [
min([v, math.copysign(self.__pose_rotation_max, v)], key=abs)
for v in shift_command[3:]
]
try:
success, potential_target_values = self._get_new_joints_w_ik(
shift_command)
except ArmCommanderException as e:
return e.status, "Error while validating pose : {}".format(
e.message)
if not success:
return CommandStatus.NO_PLAN_AVAILABLE, "Unable to find on invert kinematics for the target position"
else:
self.set_target_values(potential_target_values)
else:
# Accumulate multiple commands if they come faster than the publish rate
shift_command = [
min([v, math.copysign(self.__joints_rotation_max, v)], key=abs)
for v in shift_command
]
target_values = [
actual + shift for actual, shift in zip(
self._last_target_values, shift_command)
]
target_values = self.__limit_params_joints(target_values)
joints_to_jog = [
i for i, value in enumerate(shift_command) if value != 0
]
try:
self.__validate_params_joints(
target_values
) # validate joints according to joints limits
except ArmCommanderException as e:
return e.status, "Error while validating joints : {}".format(
e.message)
# validate target joints validity, based on collisions checking
try:
valid, link_colliding1, link_colliding2 = self.__check_joint_validity_moveit(
target_values)
if not valid:
if link_colliding1 is not None and link_colliding2 is not None:
return CommandStatus.JOG_CONTROLLER_FAILURE, \
"Joints target unreachable because of collision between {} and {}".format(
link_colliding1, link_colliding2)
else:
return CommandStatus.JOG_CONTROLLER_FAILURE, \
"Joints target unreachable because of collision between two parts of Ned"
except rospy.ServiceException as e:
return e, "Error while validating joint : {}".format(e.message)
self._current_jogged_joints = joints_to_jog
self.set_target_values(target_values)
self._new_robot_state = RobotState()
return CommandStatus.SUCCESS, "Command send"
def __callback_send_jog_joints_command(self, msg):
# check if the learning mode is false and the robot is calibrated
self.__check_before_use_jog()
shift_mode = msg.cmd
if shift_mode != self._shift_mode:
self._reset_last_pub()
self._shift_mode = shift_mode
if self._last_shift_values_cmd != msg.shift_values:
self._last_shift_values_cmd = msg.shift_values
self.__jog_errors_cpt = 0
shift_command = list(msg.shift_values)
if shift_mode == JogShiftRequest.JOINTS_SHIFT:
# Accumulate multiple commands if they come faster than the publish rate
shift_command = [
min([v, math.copysign(self.__joints_rotation_max, v)], key=abs)
for v in shift_command
]
target_values = [
actual + shift for actual, shift in zip(
self._last_target_values, shift_command)
]
# get index of the jogged joints in a list
joints_to_jog = [
i for i, value in enumerate(shift_command) if value != 0
]
target_values = self.__limit_params_joints(target_values)
try:
self.__validate_params_joints(
target_values
) # validate joints according to joints limits
except ArmCommanderException as e:
message = "Jog Controller - Error while validating joint : {}".format(
e.message)
rospy.logwarn_throttle(1, message)
return self.__publish_jog_error(
CommandStatus.JOG_CONTROLLER_FAILURE, message)
# validate target joints validity, based on collisions checking
try:
valid, link_colliding1, link_colliding2 = self.__check_joint_validity_moveit(
target_values)
if not valid:
return self.__publish_jog_error(
CommandStatus.JOG_CONTROLLER_FAILURE,
"Joints target unreachable because of collision between {} and {}"
.format(link_colliding1, link_colliding2))
except rospy.ServiceException as e:
message = "Jog Controller - Error while validating joint : {}".format(
e.message)
rospy.logwarn_throttle(1, message)
return self.__publish_jog_error(
CommandStatus.JOG_CONTROLLER_FAILURE, message)
if self.__collision_detected:
return self.__publish_jog_error(
CommandStatus.JOG_CONTROLLER_FAILURE, "Collision detected")
self.set_target_values(target_values)
self._current_jogged_joints = joints_to_jog
self._new_robot_state = RobotState()
return self.__publish_jog_error(CommandStatus.SUCCESS, "Success")
def __init__(self, parameters_validator):
# - Publisher which publishes if JogController is enabled
self._enabled = False
self._jog_enabled_publisher = rospy.Publisher(
'/niryo_robot/jog_interface/is_enabled', Bool, queue_size=1)
rospy.Timer(
rospy.Duration(1.0 / rospy.get_param("~jog_enable_publish_rate")),
self._publish_jog_enabled)
self._check_disable_jog_timer = None
self._last_command_timer = rospy.get_time()
self._publish_jog_enabled()
# Service to enable Jog Controller
rospy.Service('/niryo_robot/jog_interface/enable', SetBool,
self.__callback_enable_jog)
# - Direct publisher to joint controller
self._joint_trajectory_publisher = rospy.Publisher(
rospy.get_param("~joint_controller_name") + '/command',
JointTrajectory,
queue_size=3)
# Publishing rate
self._timer_rate = rospy.get_param("~jog_timer_rate_sec")
self._publisher_joint_trajectory_timer = None
# - Subscribers
self._joint_states = None
rospy.Subscriber('/joint_states', JointState,
self.__callback_joint_states)
self._robot_state = None
rospy.Subscriber('/niryo_robot/robot_state', RobotState,
self.__callback_sub_robot_state)
self.__learning_mode_on = None
rospy.Subscriber('/niryo_robot/learning_mode/state', Bool,
self.__callback_sub_learning_mode)
self.__hardware_status = None
rospy.Subscriber('/niryo_robot_hardware_interface/hardware_status',
HardwareStatus, self.__callback_hardware_status)
# - Service
rospy.Service('/niryo_robot/jog_interface/jog_shift_commander',
JogShift, self.__callback_jog_commander)
# - Kinematics
self._new_robot_state = RobotState()
self._last_robot_state_published = RobotState()
self._service_ik = rospy.ServiceProxy(
'/niryo_robot/kinematics/inverse', GetIK)
# - Values Init
self._shift_mode = None
self._last_target_values = [0.0 for _ in range(6)]
self._target_values = None
# Validation
self.__parameters_validator = parameters_validator
jog_limits = rospy.get_param("~jog_limits")
self.__pose_translation_max = jog_limits["translation"]
self.__pose_rotation_max = jog_limits["rotation"]
self.__joints_rotation_max = jog_limits["joints"]
# - Others param
self.__time_without_jog_limit = rospy.get_param(
"~time_without_jog_limit")
def __callback_jog_commander(self, msg):
if self.__hardware_status.calibration_needed or self.__hardware_status.calibration_in_progress:
return CommandStatus.ABORTED, "Cannot send command cause Jog because calibration is not done"
if self.__learning_mode_on:
try:
rospy.wait_for_service('/niryo_robot/learning_mode/activate',
2)
service = rospy.ServiceProxy(
'/niryo_robot/learning_mode/activate', SetBool)
result = service(True)
if result.status != CommandStatus.SUCCESS:
return CommandStatus.ABORTED, "Cannot send command cause Jog because learning mode is on and" \
" cannot be disabled"
rospy.sleep(0.1)
except (rospy.ROSException, rospy.ServiceException):
return CommandStatus.ABORTED, "Error while trying to turn Off learning mode"
if not self._enabled:
ret, str_msg = self.enable()
if ret == CommandStatus.ABORTED:
return CommandStatus.ABORTED, "Cannot send command cause Jog is not activated and cannot be"
self._last_command_timer = rospy.get_time()
shift_mode = msg.cmd
if shift_mode != self._shift_mode:
self._reset_last_pub()
self._shift_mode = shift_mode
shift_command = list(msg.shift_values)
if shift_mode == JogShiftRequest.POSE_SHIFT:
shift_command[:3] = [
min([v, math.copysign(self.__pose_translation_max, v)],
key=abs) for v in shift_command[:3]
]
shift_command[3:] = [
min([v, math.copysign(self.__pose_rotation_max, v)], key=abs)
for v in shift_command[3:]
]
try:
success, potential_target_values = self._get_new_joints_w_ik(
shift_command)
except ArmCommanderException as e:
return e.status, "Error while validating pose : {}".format(
e.message)
if not success:
return CommandStatus.NO_PLAN_AVAILABLE, "Unable to find on invert kinematics for the target position"
else:
self._target_values = potential_target_values
else:
# Accumulate multiple commands if they come faster than the publish rate
shift_command = [
min([v, math.copysign(self.__joints_rotation_max, v)], key=abs)
for v in shift_command
]
target_values = [
actual + shift for actual, shift in zip(
self._last_target_values, shift_command)
]
target_values = self.__limit_params_joints(target_values)
try:
self.__validate_params_joints(target_values)
except ArmCommanderException as e:
return e.status, "Error while validating joints : {}".format(
e.message)
self._target_values = target_values
self._new_robot_state = RobotState()
return CommandStatus.SUCCESS, "Command send"